Hiroaki Matsuda
this is program for mbed ,to relay MCUcomm data to servo
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "Servo.h" 00003 #include "command.h" 00004 00005 #define MAX_DATA 1800 00006 #define TIME_INTERRAPUT_US 16000 00007 00008 #define TARGETPOSITION_COMMAND 0x64 00009 #define SET_TARGETPOSITION_COMMAND 0x67 00010 #define SERVO_MOVE_COMMAND 0x58 00011 #define POWER_ON_COMMAND 0x59 00012 #define POWER_OFF_COMMAND 0x5A 00013 #define SENDDATA_COMMAND 0x05 00014 #define INIT_COMMAND 0x66 00015 00016 #define TARGETPOSITION_BYTE 7 00017 #define INIT_BYTE 6 00018 #define SERVO_MOVE_BYTE 3 00019 #define POWER_ONOFF_BYTE 3 00020 #define SENDDATA_BYTE 3 00021 00022 #define ACK 0x06 00023 #define NACK 0x15 00024 00025 Ticker flipper; 00026 Serial device(USBTX, USBRX); // tx, rx 00027 DigitalOut myLed1(LED1); 00028 DigitalOut myLed2(LED2); 00029 DigitalOut myLed3(LED3); 00030 00031 Command servo1(p21); 00032 Command servo2(p22); 00033 Command servo3(p23); 00034 Command servo4(p24); 00035 Command servo5(p25); 00036 Command servo6(p26); 00037 00038 AnalogIn ain1(p15); 00039 AnalogIn ain2(p16); 00040 AnalogIn ain3(p17); 00041 AnalogIn ain4(p18); 00042 AnalogIn ain5(p19); 00043 AnalogIn ain6(p20); 00044 00045 I2C i2c(p9,p10); 00046 00047 typedef struct{ 00048 short int target; 00049 short int preTarget; 00050 long perTime; 00051 bool moveFlag; 00052 int resolution; 00053 int positionNow; 00054 }servo_t; 00055 servo_t servo[7]; 00056 00057 void flip() { 00058 for(int it=1;it<7;it++){ 00059 if(servo[it].moveFlag==true){ 00060 servo[it].positionNow+=servo[it].resolution; 00061 if((servo[it].target-servo[it].preTarget > 0 && servo[it].positionNow-servo[it].target > 0) || (servo[it].target-servo[it].preTarget < 0 && servo[it].positionNow-servo[it].target < 0)){ 00062 servo[it].moveFlag=false; 00063 servo[it].preTarget = servo[it].target; 00064 servo[it].positionNow = servo[it].target; 00065 if(it == 1){ 00066 myLed1 = 0; 00067 }else if(it == 2){ 00068 myLed2 = 0; 00069 }else if(it == 3){ 00070 myLed3 = 0; 00071 } 00072 } 00073 switch(it){ 00074 case 1: 00075 servo1=(double)servo[it].positionNow/MAX_DATA; 00076 break; 00077 case 2: 00078 servo2=(double)servo[it].positionNow/MAX_DATA; 00079 break; 00080 case 3: 00081 servo3=(double)servo[it].positionNow/MAX_DATA; 00082 break; 00083 case 4: 00084 servo4=(double)servo[it].positionNow/MAX_DATA; 00085 break; 00086 case 5: 00087 servo5=(double)servo[it].positionNow/MAX_DATA; 00088 break; 00089 case 6: 00090 servo6=(double)servo[it].positionNow/MAX_DATA; 00091 break; 00092 } 00093 } 00094 } 00095 } 00096 00097 00098 int main() { 00099 device.baud(115200); 00100 flipper.attach_us(&flip, TIME_INTERRAPUT_US); 00101 // i2c.frequency(40000); 00102 char cmd[6]; 00103 char add[1]; 00104 char init[2]; 00105 add[0] = 0x01; 00106 init[0] = 0x2A; 00107 init[1] = 0x01; 00108 // while(i2c.write(0x3A,init,2)); 00109 00110 for(int it=0;it<7;it++){ 00111 servo[it].target=900; 00112 servo[it].perTime=0; 00113 servo[it].moveFlag=false; 00114 servo[it].preTarget=900; 00115 servo[it].resolution=0; 00116 servo[it].positionNow=900; 00117 } 00118 00119 myLed1=0; 00120 myLed2=0; 00121 myLed3=0; 00122 int valueNumber=0; 00123 int id=0; 00124 int command=0; 00125 int getValue=0; 00126 int checkSum=0; 00127 00128 while(1){ 00129 if(device.readable()){ 00130 valueNumber++; 00131 getValue = device.getc(); 00132 switch(valueNumber){ 00133 case 1: //get ID 00134 id = getValue; 00135 checkSum = getValue; 00136 break; 00137 case 2: //get command upbyte 00138 command = getValue; 00139 checkSum += getValue; 00140 break; 00141 case 3: 00142 case 4: 00143 case 5: 00144 case 6: 00145 case 7: 00146 //servo_move_command 00147 if(command == SERVO_MOVE_COMMAND){ 00148 if((valueNumber == SERVO_MOVE_BYTE && (checkSum & 0x0FF) == getValue) || valueNumber < SERVO_MOVE_BYTE){ 00149 if(0 < id && id < 7){ 00150 if(servo[id].moveFlag == true) 00151 servo[id].preTarget = servo[id].positionNow; 00152 servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00000)); 00153 if(servo[id].resolution != 0){ 00154 servo[id].moveFlag = true; 00155 if(id == 1) 00156 myLed1 = 1; 00157 else if(id == 2) 00158 myLed2 = 2; 00159 else if(id == 3) 00160 myLed3 = 3; 00161 } 00162 }else if(id == 0x0FE){ 00163 for(int it=1; it<7; it++){ 00164 if(servo[it].resolution != 0){ 00165 servo[it].moveFlag = true; 00166 if(it == 1) 00167 myLed1 = 1; 00168 else if(it == 2) 00169 myLed2 = 2; 00170 else if(it == 3) 00171 myLed3 = 3; 00172 } 00173 } 00174 } 00175 if(valueNumber == SERVO_MOVE_BYTE){ 00176 device.putc(id); 00177 device.putc(ACK); //ACK 00178 valueNumber = 0; 00179 } 00180 }else{ 00181 device.putc(id); 00182 device.putc(NACK); //NACK 00183 valueNumber = 0; 00184 } 00185 } 00186 00187 00188 //senddata command check 00189 if(command == SENDDATA_COMMAND){ 00190 if((valueNumber == SENDDATA_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < SENDDATA_BYTE){ 00191 myLed1 = 1; 00192 00193 /* i2c.write(0x3A,add,1); 00194 i2c.read(0x3A,cmd,6); 00195 */ 00196 00197 int send_data; 00198 switch(id){ 00199 case 1: //x 00200 // myLed1 = 1; 00201 send_data = ain1.read_u16(); 00202 device.putc(send_data >> 8); 00203 device.putc(send_data & 0xFF); 00204 break; 00205 case 2: //y 00206 send_data = ain2.read_u16(); 00207 device.putc(send_data >> 8); 00208 device.putc(send_data & 0xFF); 00209 break; 00210 case 3: //z 00211 send_data = ain3.read_u16(); 00212 device.putc(send_data >> 8); 00213 device.putc(send_data & 0xFF); 00214 break; 00215 case 4: //other 00216 send_data = ain4.read_u16(); 00217 device.putc(send_data >> 8); 00218 device.putc(send_data & 0xFF); 00219 break; 00220 case 5: //other 00221 send_data = ain5.read_u16(); 00222 device.putc(send_data >> 8); 00223 device.putc(send_data & 0xFF); 00224 break; 00225 case 6: //other 00226 send_data = ain6.read_u16(); 00227 device.putc(send_data >> 8); 00228 device.putc(send_data & 0xFF); 00229 break; 00230 default: 00231 device.putc(0); 00232 device.putc(0); 00233 device.putc(id); 00234 device.putc(NACK); //NACK 00235 valueNumber = 0; 00236 break; 00237 } 00238 00239 if(valueNumber == SENDDATA_BYTE && id<=6){ 00240 device.putc(id); 00241 device.putc(ACK); //ACK 00242 valueNumber = 0; 00243 } 00244 }else{ 00245 device.putc(0); 00246 device.putc(0); 00247 device.putc(id); 00248 device.putc(NACK); //NACK 00249 valueNumber = 0; 00250 } 00251 } 00252 00253 //power_onoff command check 00254 if(command == POWER_ON_COMMAND){ 00255 if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){ 00256 switch(id){ 00257 case 1: 00258 servo1.on(); 00259 break; 00260 case 2: 00261 servo2.on(); 00262 break; 00263 case 3: 00264 servo3.on(); 00265 break; 00266 case 4: 00267 servo4.on(); 00268 break; 00269 case 5: 00270 servo5.on(); 00271 break; 00272 case 6: 00273 servo6.on(); 00274 break; 00275 default : 00276 device.putc(id); 00277 device.putc(NACK); //NACK 00278 valueNumber = 0; 00279 break; 00280 } 00281 if(valueNumber == POWER_ONOFF_BYTE){ 00282 device.putc(id); 00283 device.putc(ACK); //ACK 00284 valueNumber = 0; 00285 } 00286 }else{ 00287 device.putc(id); 00288 device.putc(NACK); //NACK 00289 valueNumber = 0; 00290 } 00291 } 00292 00293 //power_onoff command check 00294 if(command == POWER_OFF_COMMAND){ 00295 if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){ 00296 switch(id){ 00297 case 1: 00298 servo1.off(); 00299 break; 00300 case 2: 00301 servo2.off(); 00302 break; 00303 case 3: 00304 servo3.off(); 00305 break; 00306 case 4: 00307 servo4.off(); 00308 break; 00309 case 5: 00310 servo5.off(); 00311 break; 00312 case 6: 00313 servo6.off(); 00314 break; 00315 default : 00316 device.putc(id); 00317 device.putc(NACK); //NACK 00318 valueNumber = 0; 00319 break; 00320 } 00321 if(valueNumber == POWER_ONOFF_BYTE){ 00322 device.putc(id); 00323 device.putc(ACK); //ACK 00324 valueNumber = 0; 00325 } 00326 }else{ 00327 device.putc(id); 00328 device.putc(NACK); //NACK 00329 valueNumber = 0; 00330 } 00331 } 00332 00333 //init command check 00334 if(command == INIT_COMMAND){ 00335 if((valueNumber == INIT_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < INIT_BYTE){ 00336 switch(id){ 00337 case 1: 00338 servo1.homePosition(valueNumber,getValue); 00339 break; 00340 case 2: 00341 servo2.homePosition(valueNumber,getValue); 00342 break; 00343 case 3: 00344 servo3.homePosition(valueNumber,getValue); 00345 break; 00346 case 4: 00347 servo4.homePosition(valueNumber,getValue); 00348 break; 00349 case 5: 00350 servo5.homePosition(valueNumber,getValue); 00351 break; 00352 case 6: 00353 servo6.homePosition(valueNumber,getValue); 00354 break; 00355 default : 00356 device.putc(id); 00357 device.putc(NACK); //NACK 00358 valueNumber = 0; 00359 break; 00360 } 00361 if(valueNumber == INIT_BYTE){ 00362 device.putc(id); 00363 device.putc(ACK); //ACK 00364 valueNumber = 0; 00365 } 00366 }else{ 00367 device.putc(id); 00368 device.putc(NACK); //NACK 00369 valueNumber = 0; 00370 } 00371 } 00372 00373 //move command check 00374 if(command == TARGETPOSITION_COMMAND || command == SET_TARGETPOSITION_COMMAND){ 00375 if((valueNumber == TARGETPOSITION_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < TARGETPOSITION_BYTE){ 00376 switch(id){ 00377 case 1: 00378 servo1.targetPosition(valueNumber,getValue); 00379 servo[id].target = servo1.getPosition(); 00380 servo[id].perTime = servo1.getTime(); 00381 break; 00382 case 2: 00383 servo2.targetPosition(valueNumber,getValue); 00384 servo[id].target = servo2.getPosition(); 00385 servo[id].perTime = servo2.getTime(); 00386 break; 00387 case 3: 00388 servo3.targetPosition(valueNumber,getValue); 00389 servo[id].target = servo3.getPosition(); 00390 servo[id].perTime = servo3.getTime(); 00391 break; 00392 case 4: 00393 servo4.targetPosition(valueNumber,getValue); 00394 servo[id].target = servo4.getPosition(); 00395 servo[id].perTime = servo4.getTime(); 00396 break; 00397 case 5: 00398 servo5.targetPosition(valueNumber,getValue); 00399 servo[id].target = servo5.getPosition(); 00400 servo[id].perTime = servo5.getTime(); 00401 break; 00402 case 6: 00403 servo6.targetPosition(valueNumber,getValue); 00404 servo[id].target = servo6.getPosition(); 00405 servo[id].perTime = servo6.getTime(); 00406 break; 00407 } 00408 if(valueNumber == TARGETPOSITION_BYTE){ 00409 device.putc(id); 00410 device.putc(ACK); //ACK 00411 00412 if(command == TARGETPOSITION_COMMAND){ 00413 if(servo[id].moveFlag == true) 00414 servo[id].preTarget = servo[id].positionNow; 00415 servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00000)); 00416 if(servo[id].resolution != 0){ 00417 servo[id].moveFlag = true; 00418 if(id == 1) 00419 myLed1 = 1; 00420 else if(id == 2) 00421 myLed2 = 2; 00422 else if(id == 3) 00423 myLed3 = 3; 00424 } 00425 } 00426 valueNumber = 0; 00427 } 00428 }else{ 00429 device.putc(id); 00430 device.putc(NACK); //NACK 00431 valueNumber = 0; 00432 } 00433 } 00434 //plus checkSum 00435 checkSum += getValue; 00436 break; 00437 00438 } 00439 } 00440 } 00441 } 00442
Generated on Tue Jul 19 2022 02:08:10 by
1.7.2