Maintenance: Project Hub will be unavailable on Monday 24 (9AM to 6PM CET) while we deploy critical improvements
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