Components and supplies
Jumper wires (generic)
Jumper Wires
Arduino Nano RP2040 Connect
Robotic Arm Kit
Raspberry Pi Pico
9V to 12V Battery
12V to 6V Buck Converter
Robot Base Frame / Chassis
ESP32 Camera Module Development Board
VNH3ASP30 DC Motor Driver
DC Power Jack
Robot Wheels
Geared DC Motor, 12 V
High Torque Servo Motors
Apps and platforms
Computer Aided Simulation Program (CASP)
Project description
Code
Custom Block Code
c_cpp
1//CustomBlock193083739.h 2 3#ifndef CUSTOMBLOCK193083739_H 4#define CUSTOMBLOCK193083739_H 5 6#include "sim_common_definitions.h" 7 8//BLOCK_PREPROC_DEFS_HERE 9 10class SHARED_LIB_TYPE CustomBlock193083739 11{ 12public: 13 CustomBlock193083739(); 14 ~CustomBlock193083739(); 15 void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg); 16 void PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out); 17 void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out); 18 void PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out); 19public: 20 //block internal parameter variable declaration 21 _FXDPT_FLOAT32 mouse_sen, def_x, def_y, def_z; 22 //block internal initial condition variable declaration 23private: 24 _UINT64 m_prev_tick; 25 _BYTE m_prev_led_kb, m_prev_en_kb, m_prev_park_kb; 26 int m_en_mouse; 27 int m_prev_mouse_pos[3]; 28}; 29 30#endif 31 32//CustomBlock193083739.cpp 33#include "CustomBlock193083739.h" 34#include "simcode.h" 35#include "math.h" 36#include "sim_helper.h" 37 38CustomBlock193083739::CustomBlock193083739(){ 39} 40 41CustomBlock193083739::~CustomBlock193083739(){ 42} 43 44void CustomBlock193083739::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg){ 45} 46 47void CustomBlock193083739::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){ 48 *led_out = 0; 49 *en_out = -2;//first run 50 xyz_out[0] = def_x; 51 xyz_out[1] = def_y; 52 xyz_out[2] = def_z; 53 *tw_out = 0; 54 *jaw_out = 0.5; 55 // 56 m_prev_mouse_pos[0] = kb_in[257]; 57 m_prev_mouse_pos[1] = kb_in[258]; 58 m_prev_mouse_pos[2] = kb_in[259]; 59 // 60 m_prev_tick = arg->rt_clk_tick; 61 m_prev_led_kb = m_prev_en_kb = m_prev_park_kb = 0; 62} 63 64void CustomBlock193083739::Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){ 65 if(arg->rt_clk_tick-m_prev_tick < 10000) 66 return; 67 m_prev_tick = arg->rt_clk_tick; 68 int x = kb_in[257]; 69 int y = kb_in[258]; 70 int z = kb_in[259]; 71 int diffx = x - m_prev_mouse_pos[0];m_prev_mouse_pos[0] = x; 72 int diffy = y - m_prev_mouse_pos[1];m_prev_mouse_pos[1] = y; 73 int diffz = z - m_prev_mouse_pos[2];m_prev_mouse_pos[2] = z; 74 //flash light led logic 75 if(kb_in[76])//L 76 { 77 if(m_prev_led_kb == 0) 78 { 79 m_prev_led_kb = 1; 80 *led_out = (*led_out)?0:1; 81 } 82 } 83 else 84 m_prev_led_kb = 0; 85 //park logic 86 if(*en_out > -2) 87 { 88 if(kb_in[80])//P. press P until it is parked 89 { 90 *en_out = -1; 91 } 92 else 93 { 94 //enable disable x,y,z control 95 if(kb_in[69])//E 96 { 97 if(m_prev_en_kb == 0) 98 m_prev_en_kb = 1; 99 } 100 else 101 m_prev_en_kb = 0; 102 *en_out = 1; 103 } 104 } 105 //update x,y,z 106 if(*en_out < 0) 107 { 108 xyz_out[0] = def_x; 109 xyz_out[1] = def_y; 110 xyz_out[2] = def_z; 111 // 112 *tw_out = 0; 113 *jaw_out = 0.5; 114 } 115 else if(*en_out > 0) 116 { 117 //x,y 118 if(diffx != 0 || diffy != 0) 119 { 120 //convert curren x,y to polar 121 _FXDPT_FLOAT y = j_in[0]; 122 _FXDPT_FLOAT x = j_in[1]; 123 _FXDPT_FLOAT mag = sqrt(x*x + y*y); 124 _FXDPT_FLOAT ang = 0; 125 if(!IsZero(x)) 126 ang = atan2(y,x) * MATH_RAD_TO_DEG; 127 if(diffx != 0) 128 { 129 if(kb_in[256] & 0b10) 130 { 131 //update theta 132 _FXDPT_FLOAT theta = ((_FXDPT_FLOAT)diffx * mouse_sen * ((kb_in[17]/*ctrl*/)?0.01:0.1)); 133 ang += theta; 134 //dont set to 90. sometimes solution is out of bounds 135 if(ang > 85) ang = 85; 136 if(ang < -85) ang = -85; 137 } 138 139 } 140 if(diffy != 0) 141 { 142 if(kb_in[256] & 0b10) 143 { 144 //update radius 145 _FXDPT_FLOAT radius = ((_FXDPT_FLOAT)diffy * mouse_sen * ((kb_in[17]/*ctrl*/)?0.05:0.5)); 146 mag -= radius; 147 } 148 } 149 //convert polar back to rectangular 150 if(kb_in[256] & 0b10) 151 { 152 _FXDPT_FLOAT x = mag * cos(ang * MATH_DEG_TO_RAD); 153 _FXDPT_FLOAT y = mag * sin(ang * MATH_DEG_TO_RAD); 154 xyz_out[0] = y; 155 xyz_out[1] = x; 156 } 157 } 158 //z 159 if(diffz != 0) 160 { 161 _FXDPT_FLOAT d = (_FXDPT_FLOAT)diffz * mouse_sen; 162 d *= ((kb_in[17]/*ctrl*/)?0.01:0.1); 163 xyz_out[2] = j_in[2]-d; 164 } 165 } 166 //twist control 167 if(kb_in[84])//T 168 { 169 *tw_out += 1; 170 *tw_out = (*tw_out > 90)?90:*tw_out; 171 } 172 else if(kb_in[89])//Y 173 { 174 *tw_out -= 1; 175 *tw_out = (*tw_out < -90)?-90:*tw_out; 176 } 177 //jaw control 178 if(kb_in[74])//J 179 { 180 *jaw_out += (kb_in[16]/*shift*/)?0.1:0.01; 181 *jaw_out = (*jaw_out > 1)?1:*jaw_out; 182 } 183 else if(kb_in[75])//K 184 { 185 *jaw_out -= (kb_in[16]/*shift*/)?0.1:0.01; 186 *jaw_out = (*jaw_out < 0)?0:*jaw_out; 187 } 188 if(*en_out == -2) *en_out = -1; 189} 190 191void CustomBlock193083739::PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){ 192} 193
Downloadable files
Connection Diagram for Arduino Nano RP2040 Connect
Connection Diagram for Arduino Nano RP2040 Connect
Connection Diagram for Raspberry Pi Pico W
Connection Diagram for Raspberry Pi Pico W
Comments
Only logged in users can leave comments