Hiroaki Matsuda / Mbed 2 deprecated mbed-for-McuComm

this is program for mbed ,to relay MCUcomm data to servo

Dependencies:   Servo mbed

main.cpp@1:0fe52e356d01, 2012-12-05 (annotated)

Committer:
matsu
Date:
Wed Dec 05 15:13:57 2012 +0000
Revision:
1:0fe52e356d01
Parent:
0:09bf8e3091e8 
if you send signal from pc to mbed ,servo motor will work that you want

Who changed what in which revision?

UserRevisionLine numberNew contents of line
matsu 0:09bf8e3091e8 1 #include "mbed.h"
matsu 0:09bf8e3091e8 2 #include "Servo.h"
matsu 0:09bf8e3091e8 3 #include "command.h"
matsu 0:09bf8e3091e8 4
matsu 0:09bf8e3091e8 5 #define MAX_DATA 1800
matsu 0:09bf8e3091e8 6 #define TIME_INTERRAPUT_US 16000
matsu 0:09bf8e3091e8 7
matsu 1:0fe52e356d01 8 #define TARGETPOSITION_COMMAND 0x64
matsu 1:0fe52e356d01 9 #define SET_TARGETPOSITION_COMMAND 0x67
matsu 0:09bf8e3091e8 10 #define SERVO_MOVE_COMMAND 0x58
matsu 1:0fe52e356d01 11 #define POWER_ON_COMMAND 0x59
matsu 1:0fe52e356d01 12 #define POWER_OFF_COMMAND 0x5A
matsu 0:09bf8e3091e8 13 #define SENDDATA_COMMAND 0x05
matsu 0:09bf8e3091e8 14 #define INIT_COMMAND 0x66
matsu 0:09bf8e3091e8 15
matsu 1:0fe52e356d01 16 #define TARGETPOSITION_BYTE 7
matsu 1:0fe52e356d01 17 #define INIT_BYTE 6
matsu 0:09bf8e3091e8 18 #define SERVO_MOVE_BYTE 3
matsu 0:09bf8e3091e8 19 #define POWER_ONOFF_BYTE 3
matsu 0:09bf8e3091e8 20 #define SENDDATA_BYTE 3
matsu 0:09bf8e3091e8 21
matsu 0:09bf8e3091e8 22 #define ACK 0x06
matsu 0:09bf8e3091e8 23 #define NACK 0x15
matsu 0:09bf8e3091e8 24
matsu 0:09bf8e3091e8 25 Ticker flipper;
matsu 0:09bf8e3091e8 26 Serial device(USBTX, USBRX); // tx, rx
matsu 0:09bf8e3091e8 27 DigitalOut myLed1(LED1);
matsu 0:09bf8e3091e8 28 DigitalOut myLed2(LED2);
matsu 0:09bf8e3091e8 29 DigitalOut myLed3(LED3);
matsu 0:09bf8e3091e8 30
matsu 0:09bf8e3091e8 31 Command servo1(p21);
matsu 0:09bf8e3091e8 32 Command servo2(p22);
matsu 0:09bf8e3091e8 33 Command servo3(p23);
matsu 0:09bf8e3091e8 34 Command servo4(p24);
matsu 0:09bf8e3091e8 35 Command servo5(p25);
matsu 0:09bf8e3091e8 36 Command servo6(p26);
matsu 0:09bf8e3091e8 37
matsu 0:09bf8e3091e8 38 AnalogIn ain1(p15);
matsu 0:09bf8e3091e8 39 AnalogIn ain2(p16);
matsu 0:09bf8e3091e8 40 AnalogIn ain3(p17);
matsu 0:09bf8e3091e8 41 AnalogIn ain4(p18);
matsu 0:09bf8e3091e8 42 AnalogIn ain5(p19);
matsu 0:09bf8e3091e8 43 AnalogIn ain6(p20);
matsu 0:09bf8e3091e8 44
matsu 1:0fe52e356d01 45 I2C i2c(p9,p10);
matsu 1:0fe52e356d01 46
matsu 0:09bf8e3091e8 47 typedef struct{
matsu 0:09bf8e3091e8 48 short int target;
matsu 0:09bf8e3091e8 49 short int preTarget;
matsu 0:09bf8e3091e8 50 long perTime;
matsu 0:09bf8e3091e8 51 bool moveFlag;
matsu 0:09bf8e3091e8 52 int resolution;
matsu 1:0fe52e356d01 53 int positionNow;
matsu 0:09bf8e3091e8 54 }servo_t;
matsu 0:09bf8e3091e8 55 servo_t servo[7];
matsu 0:09bf8e3091e8 56
matsu 0:09bf8e3091e8 57 void flip() {
matsu 0:09bf8e3091e8 58 for(int it=1;it<7;it++){
matsu 0:09bf8e3091e8 59 if(servo[it].moveFlag==true){
matsu 0:09bf8e3091e8 60 servo[it].positionNow+=servo[it].resolution;
matsu 1:0fe52e356d01 61 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)){
matsu 0:09bf8e3091e8 62 servo[it].moveFlag=false;
matsu 0:09bf8e3091e8 63 servo[it].preTarget = servo[it].target;
matsu 0:09bf8e3091e8 64 servo[it].positionNow = servo[it].target;
matsu 0:09bf8e3091e8 65 if(it == 1){
matsu 0:09bf8e3091e8 66 myLed1 = 0;
matsu 0:09bf8e3091e8 67 }else if(it == 2){
matsu 0:09bf8e3091e8 68 myLed2 = 0;
matsu 0:09bf8e3091e8 69 }else if(it == 3){
matsu 0:09bf8e3091e8 70 myLed3 = 0;
matsu 0:09bf8e3091e8 71 }
matsu 0:09bf8e3091e8 72 }
matsu 0:09bf8e3091e8 73 switch(it){
matsu 0:09bf8e3091e8 74 case 1:
matsu 1:0fe52e356d01 75 servo1=(double)servo[it].positionNow/MAX_DATA;
matsu 0:09bf8e3091e8 76 break;
matsu 0:09bf8e3091e8 77 case 2:
matsu 1:0fe52e356d01 78 servo2=(double)servo[it].positionNow/MAX_DATA;
matsu 0:09bf8e3091e8 79 break;
matsu 0:09bf8e3091e8 80 case 3:
matsu 1:0fe52e356d01 81 servo3=(double)servo[it].positionNow/MAX_DATA;
matsu 0:09bf8e3091e8 82 break;
matsu 0:09bf8e3091e8 83 case 4:
matsu 1:0fe52e356d01 84 servo4=(double)servo[it].positionNow/MAX_DATA;
matsu 0:09bf8e3091e8 85 break;
matsu 0:09bf8e3091e8 86 case 5:
matsu 1:0fe52e356d01 87 servo5=(double)servo[it].positionNow/MAX_DATA;
matsu 0:09bf8e3091e8 88 break;
matsu 0:09bf8e3091e8 89 case 6:
matsu 1:0fe52e356d01 90 servo6=(double)servo[it].positionNow/MAX_DATA;
matsu 0:09bf8e3091e8 91 break;
matsu 0:09bf8e3091e8 92 }
matsu 0:09bf8e3091e8 93 }
matsu 0:09bf8e3091e8 94 }
matsu 0:09bf8e3091e8 95 }
matsu 0:09bf8e3091e8 96
matsu 0:09bf8e3091e8 97
matsu 0:09bf8e3091e8 98 int main() {
matsu 0:09bf8e3091e8 99 device.baud(115200);
matsu 0:09bf8e3091e8 100 flipper.attach_us(&flip, TIME_INTERRAPUT_US);
matsu 1:0fe52e356d01 101 // i2c.frequency(40000);
matsu 1:0fe52e356d01 102 char cmd[6];
matsu 1:0fe52e356d01 103 char add[1];
matsu 1:0fe52e356d01 104 char init[2];
matsu 1:0fe52e356d01 105 add[0] = 0x01;
matsu 1:0fe52e356d01 106 init[0] = 0x2A;
matsu 1:0fe52e356d01 107 init[1] = 0x01;
matsu 1:0fe52e356d01 108 // while(i2c.write(0x3A,init,2));
matsu 0:09bf8e3091e8 109
matsu 0:09bf8e3091e8 110 for(int it=0;it<7;it++){
matsu 0:09bf8e3091e8 111 servo[it].target=900;
matsu 0:09bf8e3091e8 112 servo[it].perTime=0;
matsu 0:09bf8e3091e8 113 servo[it].moveFlag=false;
matsu 0:09bf8e3091e8 114 servo[it].preTarget=900;
matsu 0:09bf8e3091e8 115 servo[it].resolution=0;
matsu 0:09bf8e3091e8 116 servo[it].positionNow=900;
matsu 0:09bf8e3091e8 117 }
matsu 0:09bf8e3091e8 118
matsu 0:09bf8e3091e8 119 myLed1=0;
matsu 0:09bf8e3091e8 120 myLed2=0;
matsu 0:09bf8e3091e8 121 myLed3=0;
matsu 0:09bf8e3091e8 122 int valueNumber=0;
matsu 0:09bf8e3091e8 123 int id=0;
matsu 0:09bf8e3091e8 124 int command=0;
matsu 0:09bf8e3091e8 125 int getValue=0;
matsu 0:09bf8e3091e8 126 int checkSum=0;
matsu 1:0fe52e356d01 127
matsu 0:09bf8e3091e8 128 while(1){
matsu 0:09bf8e3091e8 129 if(device.readable()){
matsu 0:09bf8e3091e8 130 valueNumber++;
matsu 1:0fe52e356d01 131 getValue = device.getc();
matsu 0:09bf8e3091e8 132 switch(valueNumber){
matsu 0:09bf8e3091e8 133 case 1: //get ID
matsu 0:09bf8e3091e8 134 id = getValue;
matsu 0:09bf8e3091e8 135 checkSum = getValue;
matsu 0:09bf8e3091e8 136 break;
matsu 0:09bf8e3091e8 137 case 2: //get command upbyte
matsu 0:09bf8e3091e8 138 command = getValue;
matsu 0:09bf8e3091e8 139 checkSum += getValue;
matsu 0:09bf8e3091e8 140 break;
matsu 0:09bf8e3091e8 141 case 3:
matsu 0:09bf8e3091e8 142 case 4:
matsu 0:09bf8e3091e8 143 case 5:
matsu 0:09bf8e3091e8 144 case 6:
matsu 0:09bf8e3091e8 145 case 7:
matsu 0:09bf8e3091e8 146 //servo_move_command
matsu 0:09bf8e3091e8 147 if(command == SERVO_MOVE_COMMAND){
matsu 0:09bf8e3091e8 148 if((valueNumber == SERVO_MOVE_BYTE && (checkSum & 0x0FF) == getValue) || valueNumber < SERVO_MOVE_BYTE){
matsu 0:09bf8e3091e8 149 if(0 < id && id < 7){
matsu 1:0fe52e356d01 150 if(servo[id].moveFlag == true)
matsu 1:0fe52e356d01 151 servo[id].preTarget = servo[id].positionNow;
matsu 1:0fe52e356d01 152 servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00000));
matsu 1:0fe52e356d01 153 if(servo[id].resolution != 0){
matsu 1:0fe52e356d01 154 servo[id].moveFlag = true;
matsu 1:0fe52e356d01 155 if(id == 1)
matsu 1:0fe52e356d01 156 myLed1 = 1;
matsu 1:0fe52e356d01 157 else if(id == 2)
matsu 1:0fe52e356d01 158 myLed2 = 2;
matsu 1:0fe52e356d01 159 else if(id == 3)
matsu 1:0fe52e356d01 160 myLed3 = 3;
matsu 1:0fe52e356d01 161 }
matsu 0:09bf8e3091e8 162 }else if(id == 0x0FE){
matsu 0:09bf8e3091e8 163 for(int it=1; it<7; it++){
matsu 1:0fe52e356d01 164 if(servo[it].resolution != 0){
matsu 1:0fe52e356d01 165 servo[it].moveFlag = true;
matsu 1:0fe52e356d01 166 if(it == 1)
matsu 1:0fe52e356d01 167 myLed1 = 1;
matsu 1:0fe52e356d01 168 else if(it == 2)
matsu 1:0fe52e356d01 169 myLed2 = 2;
matsu 1:0fe52e356d01 170 else if(it == 3)
matsu 1:0fe52e356d01 171 myLed3 = 3;
matsu 1:0fe52e356d01 172 }
matsu 0:09bf8e3091e8 173 }
matsu 0:09bf8e3091e8 174 }
matsu 0:09bf8e3091e8 175 if(valueNumber == SERVO_MOVE_BYTE){
matsu 0:09bf8e3091e8 176 device.putc(id);
matsu 0:09bf8e3091e8 177 device.putc(ACK); //ACK
matsu 0:09bf8e3091e8 178 valueNumber = 0;
matsu 0:09bf8e3091e8 179 }
matsu 0:09bf8e3091e8 180 }else{
matsu 0:09bf8e3091e8 181 device.putc(id);
matsu 0:09bf8e3091e8 182 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 183 valueNumber = 0;
matsu 0:09bf8e3091e8 184 }
matsu 0:09bf8e3091e8 185 }
matsu 0:09bf8e3091e8 186
matsu 0:09bf8e3091e8 187
matsu 0:09bf8e3091e8 188 //senddata command check
matsu 0:09bf8e3091e8 189 if(command == SENDDATA_COMMAND){
matsu 0:09bf8e3091e8 190 if((valueNumber == SENDDATA_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < SENDDATA_BYTE){
matsu 1:0fe52e356d01 191 myLed1 = 1;
matsu 1:0fe52e356d01 192
matsu 1:0fe52e356d01 193 /* i2c.write(0x3A,add,1);
matsu 1:0fe52e356d01 194 i2c.read(0x3A,cmd,6);
matsu 1:0fe52e356d01 195 */
matsu 1:0fe52e356d01 196
matsu 1:0fe52e356d01 197 int send_data;
matsu 0:09bf8e3091e8 198 switch(id){
matsu 0:09bf8e3091e8 199 case 1: //x
matsu 1:0fe52e356d01 200 // myLed1 = 1;
matsu 0:09bf8e3091e8 201 send_data = ain1.read_u16();
matsu 0:09bf8e3091e8 202 device.putc(send_data >> 8);
matsu 0:09bf8e3091e8 203 device.putc(send_data & 0xFF);
matsu 0:09bf8e3091e8 204 break;
matsu 0:09bf8e3091e8 205 case 2: //y
matsu 0:09bf8e3091e8 206 send_data = ain2.read_u16();
matsu 0:09bf8e3091e8 207 device.putc(send_data >> 8);
matsu 0:09bf8e3091e8 208 device.putc(send_data & 0xFF);
matsu 0:09bf8e3091e8 209 break;
matsu 0:09bf8e3091e8 210 case 3: //z
matsu 0:09bf8e3091e8 211 send_data = ain3.read_u16();
matsu 0:09bf8e3091e8 212 device.putc(send_data >> 8);
matsu 0:09bf8e3091e8 213 device.putc(send_data & 0xFF);
matsu 0:09bf8e3091e8 214 break;
matsu 0:09bf8e3091e8 215 case 4: //other
matsu 0:09bf8e3091e8 216 send_data = ain4.read_u16();
matsu 0:09bf8e3091e8 217 device.putc(send_data >> 8);
matsu 0:09bf8e3091e8 218 device.putc(send_data & 0xFF);
matsu 0:09bf8e3091e8 219 break;
matsu 0:09bf8e3091e8 220 case 5: //other
matsu 0:09bf8e3091e8 221 send_data = ain5.read_u16();
matsu 0:09bf8e3091e8 222 device.putc(send_data >> 8);
matsu 0:09bf8e3091e8 223 device.putc(send_data & 0xFF);
matsu 0:09bf8e3091e8 224 break;
matsu 0:09bf8e3091e8 225 case 6: //other
matsu 0:09bf8e3091e8 226 send_data = ain6.read_u16();
matsu 0:09bf8e3091e8 227 device.putc(send_data >> 8);
matsu 0:09bf8e3091e8 228 device.putc(send_data & 0xFF);
matsu 0:09bf8e3091e8 229 break;
matsu 0:09bf8e3091e8 230 default:
matsu 1:0fe52e356d01 231 device.putc(0);
matsu 1:0fe52e356d01 232 device.putc(0);
matsu 0:09bf8e3091e8 233 device.putc(id);
matsu 0:09bf8e3091e8 234 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 235 valueNumber = 0;
matsu 0:09bf8e3091e8 236 break;
matsu 0:09bf8e3091e8 237 }
matsu 1:0fe52e356d01 238
matsu 1:0fe52e356d01 239 if(valueNumber == SENDDATA_BYTE && id<=6){
matsu 1:0fe52e356d01 240 device.putc(id);
matsu 1:0fe52e356d01 241 device.putc(ACK); //ACK
matsu 1:0fe52e356d01 242 valueNumber = 0;
matsu 1:0fe52e356d01 243 }
matsu 1:0fe52e356d01 244 }else{
matsu 1:0fe52e356d01 245 device.putc(0);
matsu 1:0fe52e356d01 246 device.putc(0);
matsu 1:0fe52e356d01 247 device.putc(id);
matsu 1:0fe52e356d01 248 device.putc(NACK); //NACK
matsu 1:0fe52e356d01 249 valueNumber = 0;
matsu 1:0fe52e356d01 250 }
matsu 1:0fe52e356d01 251 }
matsu 1:0fe52e356d01 252
matsu 1:0fe52e356d01 253 //power_onoff command check
matsu 1:0fe52e356d01 254 if(command == POWER_ON_COMMAND){
matsu 1:0fe52e356d01 255 if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){
matsu 1:0fe52e356d01 256 switch(id){
matsu 1:0fe52e356d01 257 case 1:
matsu 1:0fe52e356d01 258 servo1.on();
matsu 1:0fe52e356d01 259 break;
matsu 1:0fe52e356d01 260 case 2:
matsu 1:0fe52e356d01 261 servo2.on();
matsu 1:0fe52e356d01 262 break;
matsu 1:0fe52e356d01 263 case 3:
matsu 1:0fe52e356d01 264 servo3.on();
matsu 1:0fe52e356d01 265 break;
matsu 1:0fe52e356d01 266 case 4:
matsu 1:0fe52e356d01 267 servo4.on();
matsu 1:0fe52e356d01 268 break;
matsu 1:0fe52e356d01 269 case 5:
matsu 1:0fe52e356d01 270 servo5.on();
matsu 1:0fe52e356d01 271 break;
matsu 1:0fe52e356d01 272 case 6:
matsu 1:0fe52e356d01 273 servo6.on();
matsu 1:0fe52e356d01 274 break;
matsu 1:0fe52e356d01 275 default :
matsu 1:0fe52e356d01 276 device.putc(id);
matsu 1:0fe52e356d01 277 device.putc(NACK); //NACK
matsu 1:0fe52e356d01 278 valueNumber = 0;
matsu 1:0fe52e356d01 279 break;
matsu 1:0fe52e356d01 280 }
matsu 1:0fe52e356d01 281 if(valueNumber == POWER_ONOFF_BYTE){
matsu 0:09bf8e3091e8 282 device.putc(id);
matsu 0:09bf8e3091e8 283 device.putc(ACK); //ACK
matsu 0:09bf8e3091e8 284 valueNumber = 0;
matsu 0:09bf8e3091e8 285 }
matsu 0:09bf8e3091e8 286 }else{
matsu 0:09bf8e3091e8 287 device.putc(id);
matsu 0:09bf8e3091e8 288 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 289 valueNumber = 0;
matsu 0:09bf8e3091e8 290 }
matsu 1:0fe52e356d01 291 }
matsu 1:0fe52e356d01 292
matsu 0:09bf8e3091e8 293 //power_onoff command check
matsu 1:0fe52e356d01 294 if(command == POWER_OFF_COMMAND){
matsu 0:09bf8e3091e8 295 if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){
matsu 0:09bf8e3091e8 296 switch(id){
matsu 0:09bf8e3091e8 297 case 1:
matsu 1:0fe52e356d01 298 servo1.off();
matsu 0:09bf8e3091e8 299 break;
matsu 0:09bf8e3091e8 300 case 2:
matsu 1:0fe52e356d01 301 servo2.off();
matsu 0:09bf8e3091e8 302 break;
matsu 0:09bf8e3091e8 303 case 3:
matsu 1:0fe52e356d01 304 servo3.off();
matsu 0:09bf8e3091e8 305 break;
matsu 0:09bf8e3091e8 306 case 4:
matsu 1:0fe52e356d01 307 servo4.off();
matsu 0:09bf8e3091e8 308 break;
matsu 0:09bf8e3091e8 309 case 5:
matsu 1:0fe52e356d01 310 servo5.off();
matsu 0:09bf8e3091e8 311 break;
matsu 0:09bf8e3091e8 312 case 6:
matsu 1:0fe52e356d01 313 servo6.off();
matsu 0:09bf8e3091e8 314 break;
matsu 0:09bf8e3091e8 315 default :
matsu 0:09bf8e3091e8 316 device.putc(id);
matsu 0:09bf8e3091e8 317 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 318 valueNumber = 0;
matsu 0:09bf8e3091e8 319 break;
matsu 0:09bf8e3091e8 320 }
matsu 0:09bf8e3091e8 321 if(valueNumber == POWER_ONOFF_BYTE){
matsu 0:09bf8e3091e8 322 device.putc(id);
matsu 0:09bf8e3091e8 323 device.putc(ACK); //ACK
matsu 0:09bf8e3091e8 324 valueNumber = 0;
matsu 0:09bf8e3091e8 325 }
matsu 0:09bf8e3091e8 326 }else{
matsu 0:09bf8e3091e8 327 device.putc(id);
matsu 0:09bf8e3091e8 328 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 329 valueNumber = 0;
matsu 0:09bf8e3091e8 330 }
matsu 0:09bf8e3091e8 331 }
matsu 0:09bf8e3091e8 332
matsu 0:09bf8e3091e8 333 //init command check
matsu 0:09bf8e3091e8 334 if(command == INIT_COMMAND){
matsu 0:09bf8e3091e8 335 if((valueNumber == INIT_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < INIT_BYTE){
matsu 0:09bf8e3091e8 336 switch(id){
matsu 0:09bf8e3091e8 337 case 1:
matsu 0:09bf8e3091e8 338 servo1.homePosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 339 break;
matsu 0:09bf8e3091e8 340 case 2:
matsu 0:09bf8e3091e8 341 servo2.homePosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 342 break;
matsu 0:09bf8e3091e8 343 case 3:
matsu 0:09bf8e3091e8 344 servo3.homePosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 345 break;
matsu 0:09bf8e3091e8 346 case 4:
matsu 0:09bf8e3091e8 347 servo4.homePosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 348 break;
matsu 0:09bf8e3091e8 349 case 5:
matsu 0:09bf8e3091e8 350 servo5.homePosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 351 break;
matsu 0:09bf8e3091e8 352 case 6:
matsu 0:09bf8e3091e8 353 servo6.homePosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 354 break;
matsu 0:09bf8e3091e8 355 default :
matsu 0:09bf8e3091e8 356 device.putc(id);
matsu 0:09bf8e3091e8 357 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 358 valueNumber = 0;
matsu 0:09bf8e3091e8 359 break;
matsu 0:09bf8e3091e8 360 }
matsu 0:09bf8e3091e8 361 if(valueNumber == INIT_BYTE){
matsu 0:09bf8e3091e8 362 device.putc(id);
matsu 0:09bf8e3091e8 363 device.putc(ACK); //ACK
matsu 0:09bf8e3091e8 364 valueNumber = 0;
matsu 0:09bf8e3091e8 365 }
matsu 0:09bf8e3091e8 366 }else{
matsu 0:09bf8e3091e8 367 device.putc(id);
matsu 0:09bf8e3091e8 368 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 369 valueNumber = 0;
matsu 0:09bf8e3091e8 370 }
matsu 0:09bf8e3091e8 371 }
matsu 0:09bf8e3091e8 372
matsu 0:09bf8e3091e8 373 //move command check
matsu 1:0fe52e356d01 374 if(command == TARGETPOSITION_COMMAND || command == SET_TARGETPOSITION_COMMAND){
matsu 1:0fe52e356d01 375 if((valueNumber == TARGETPOSITION_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < TARGETPOSITION_BYTE){
matsu 0:09bf8e3091e8 376 switch(id){
matsu 0:09bf8e3091e8 377 case 1:
matsu 0:09bf8e3091e8 378 servo1.targetPosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 379 servo[id].target = servo1.getPosition();
matsu 0:09bf8e3091e8 380 servo[id].perTime = servo1.getTime();
matsu 0:09bf8e3091e8 381 break;
matsu 0:09bf8e3091e8 382 case 2:
matsu 0:09bf8e3091e8 383 servo2.targetPosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 384 servo[id].target = servo2.getPosition();
matsu 0:09bf8e3091e8 385 servo[id].perTime = servo2.getTime();
matsu 0:09bf8e3091e8 386 break;
matsu 0:09bf8e3091e8 387 case 3:
matsu 0:09bf8e3091e8 388 servo3.targetPosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 389 servo[id].target = servo3.getPosition();
matsu 0:09bf8e3091e8 390 servo[id].perTime = servo3.getTime();
matsu 0:09bf8e3091e8 391 break;
matsu 0:09bf8e3091e8 392 case 4:
matsu 0:09bf8e3091e8 393 servo4.targetPosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 394 servo[id].target = servo4.getPosition();
matsu 0:09bf8e3091e8 395 servo[id].perTime = servo4.getTime();
matsu 0:09bf8e3091e8 396 break;
matsu 0:09bf8e3091e8 397 case 5:
matsu 0:09bf8e3091e8 398 servo5.targetPosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 399 servo[id].target = servo5.getPosition();
matsu 0:09bf8e3091e8 400 servo[id].perTime = servo5.getTime();
matsu 0:09bf8e3091e8 401 break;
matsu 0:09bf8e3091e8 402 case 6:
matsu 0:09bf8e3091e8 403 servo6.targetPosition(valueNumber,getValue);
matsu 0:09bf8e3091e8 404 servo[id].target = servo6.getPosition();
matsu 0:09bf8e3091e8 405 servo[id].perTime = servo6.getTime();
matsu 0:09bf8e3091e8 406 break;
matsu 0:09bf8e3091e8 407 }
matsu 1:0fe52e356d01 408 if(valueNumber == TARGETPOSITION_BYTE){
matsu 0:09bf8e3091e8 409 device.putc(id);
matsu 0:09bf8e3091e8 410 device.putc(ACK); //ACK
matsu 1:0fe52e356d01 411
matsu 1:0fe52e356d01 412 if(command == TARGETPOSITION_COMMAND){
matsu 1:0fe52e356d01 413 if(servo[id].moveFlag == true)
matsu 1:0fe52e356d01 414 servo[id].preTarget = servo[id].positionNow;
matsu 1:0fe52e356d01 415 servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00000));
matsu 1:0fe52e356d01 416 if(servo[id].resolution != 0){
matsu 1:0fe52e356d01 417 servo[id].moveFlag = true;
matsu 1:0fe52e356d01 418 if(id == 1)
matsu 1:0fe52e356d01 419 myLed1 = 1;
matsu 1:0fe52e356d01 420 else if(id == 2)
matsu 1:0fe52e356d01 421 myLed2 = 2;
matsu 1:0fe52e356d01 422 else if(id == 3)
matsu 1:0fe52e356d01 423 myLed3 = 3;
matsu 1:0fe52e356d01 424 }
matsu 0:09bf8e3091e8 425 }
matsu 0:09bf8e3091e8 426 valueNumber = 0;
matsu 0:09bf8e3091e8 427 }
matsu 0:09bf8e3091e8 428 }else{
matsu 0:09bf8e3091e8 429 device.putc(id);
matsu 0:09bf8e3091e8 430 device.putc(NACK); //NACK
matsu 0:09bf8e3091e8 431 valueNumber = 0;
matsu 0:09bf8e3091e8 432 }
matsu 0:09bf8e3091e8 433 }
matsu 0:09bf8e3091e8 434 //plus checkSum
matsu 0:09bf8e3091e8 435 checkSum += getValue;
matsu 0:09bf8e3091e8 436 break;
matsu 0:09bf8e3091e8 437
matsu 0:09bf8e3091e8 438 }
matsu 0:09bf8e3091e8 439 }
matsu 0:09bf8e3091e8 440 }
matsu 0:09bf8e3091e8 441 }
matsu 0:09bf8e3091e8 442