Hiroaki Matsuda
/
mbed-for-McuComm
this is program for mbed ,to relay MCUcomm data to servo
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?
User | Revision | Line number | New 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 |