Hiroaki Matsuda
/
mbed-for-McuComm
this is program for mbed ,to relay MCUcomm data to servo
main.cpp@0:09bf8e3091e8, 2012-11-28 (annotated)
- Committer:
- matsu
- Date:
- Wed Nov 28 17:12:00 2012 +0000
- Revision:
- 0:09bf8e3091e8
- Child:
- 1:0fe52e356d01
to move servo using data from pc by serial; ; ver 1
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 | #include "iSerial.h" |
matsu | 0:09bf8e3091e8 | 5 | |
matsu | 0:09bf8e3091e8 | 6 | #define MAX_DATA 1800 |
matsu | 0:09bf8e3091e8 | 7 | #define TIME_INTERRAPUT_US 16000 |
matsu | 0:09bf8e3091e8 | 8 | |
matsu | 0:09bf8e3091e8 | 9 | #define targetPosition_COMMAND 0x64 |
matsu | 0:09bf8e3091e8 | 10 | #define SET_targetPosition_COMMAND 0x67 |
matsu | 0:09bf8e3091e8 | 11 | #define SERVO_MOVE_COMMAND 0x58 |
matsu | 0:09bf8e3091e8 | 12 | #define POWER_ONOFF_COMMAND 0x59 |
matsu | 0:09bf8e3091e8 | 13 | #define SENDDATA_COMMAND 0x05 |
matsu | 0:09bf8e3091e8 | 14 | #define INIT_COMMAND 0x66 |
matsu | 0:09bf8e3091e8 | 15 | |
matsu | 0:09bf8e3091e8 | 16 | #define targetPosition_BYTE 7 |
matsu | 0:09bf8e3091e8 | 17 | #define SERVO_MOVE_BYTE 3 |
matsu | 0:09bf8e3091e8 | 18 | #define POWER_ONOFF_BYTE 3 |
matsu | 0:09bf8e3091e8 | 19 | #define SENDDATA_BYTE 3 |
matsu | 0:09bf8e3091e8 | 20 | #define INIT_BYTE 6 |
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 | 0:09bf8e3091e8 | 45 | typedef struct{ |
matsu | 0:09bf8e3091e8 | 46 | short int target; |
matsu | 0:09bf8e3091e8 | 47 | short int preTarget; |
matsu | 0:09bf8e3091e8 | 48 | long perTime; |
matsu | 0:09bf8e3091e8 | 49 | bool moveFlag; |
matsu | 0:09bf8e3091e8 | 50 | int resolution; |
matsu | 0:09bf8e3091e8 | 51 | double positionNow; |
matsu | 0:09bf8e3091e8 | 52 | }servo_t; |
matsu | 0:09bf8e3091e8 | 53 | servo_t servo[7]; |
matsu | 0:09bf8e3091e8 | 54 | |
matsu | 0:09bf8e3091e8 | 55 | void flip() { |
matsu | 0:09bf8e3091e8 | 56 | for(int it=1;it<7;it++){ |
matsu | 0:09bf8e3091e8 | 57 | if(servo[it].moveFlag==true){ |
matsu | 0:09bf8e3091e8 | 58 | servo[it].positionNow+=servo[it].resolution; |
matsu | 0:09bf8e3091e8 | 59 | 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 | 60 | servo[it].moveFlag=false; |
matsu | 0:09bf8e3091e8 | 61 | servo[it].preTarget = servo[it].target; |
matsu | 0:09bf8e3091e8 | 62 | servo[it].positionNow = servo[it].target; |
matsu | 0:09bf8e3091e8 | 63 | if(it == 1){ |
matsu | 0:09bf8e3091e8 | 64 | myLed1 = 0; |
matsu | 0:09bf8e3091e8 | 65 | }else if(it == 2){ |
matsu | 0:09bf8e3091e8 | 66 | myLed2 = 0; |
matsu | 0:09bf8e3091e8 | 67 | }else if(it == 3){ |
matsu | 0:09bf8e3091e8 | 68 | myLed3 = 0; |
matsu | 0:09bf8e3091e8 | 69 | } |
matsu | 0:09bf8e3091e8 | 70 | } |
matsu | 0:09bf8e3091e8 | 71 | switch(it){ |
matsu | 0:09bf8e3091e8 | 72 | case 1: |
matsu | 0:09bf8e3091e8 | 73 | servo1=servo[it].positionNow/MAX_DATA; |
matsu | 0:09bf8e3091e8 | 74 | break; |
matsu | 0:09bf8e3091e8 | 75 | case 2: |
matsu | 0:09bf8e3091e8 | 76 | servo2=servo[it].positionNow/MAX_DATA; |
matsu | 0:09bf8e3091e8 | 77 | break; |
matsu | 0:09bf8e3091e8 | 78 | case 3: |
matsu | 0:09bf8e3091e8 | 79 | servo3=servo[it].positionNow/MAX_DATA; |
matsu | 0:09bf8e3091e8 | 80 | break; |
matsu | 0:09bf8e3091e8 | 81 | case 4: |
matsu | 0:09bf8e3091e8 | 82 | servo4=servo[it].positionNow/MAX_DATA; |
matsu | 0:09bf8e3091e8 | 83 | break; |
matsu | 0:09bf8e3091e8 | 84 | case 5: |
matsu | 0:09bf8e3091e8 | 85 | servo5=servo[it].positionNow/MAX_DATA; |
matsu | 0:09bf8e3091e8 | 86 | break; |
matsu | 0:09bf8e3091e8 | 87 | case 6: |
matsu | 0:09bf8e3091e8 | 88 | servo6=servo[it].positionNow/MAX_DATA; |
matsu | 0:09bf8e3091e8 | 89 | break; |
matsu | 0:09bf8e3091e8 | 90 | } |
matsu | 0:09bf8e3091e8 | 91 | } |
matsu | 0:09bf8e3091e8 | 92 | } |
matsu | 0:09bf8e3091e8 | 93 | } |
matsu | 0:09bf8e3091e8 | 94 | |
matsu | 0:09bf8e3091e8 | 95 | |
matsu | 0:09bf8e3091e8 | 96 | int main() { |
matsu | 0:09bf8e3091e8 | 97 | device.baud(115200); |
matsu | 0:09bf8e3091e8 | 98 | flipper.attach_us(&flip, TIME_INTERRAPUT_US); |
matsu | 0:09bf8e3091e8 | 99 | |
matsu | 0:09bf8e3091e8 | 100 | // flag=false; |
matsu | 0:09bf8e3091e8 | 101 | for(int it=0;it<7;it++){ |
matsu | 0:09bf8e3091e8 | 102 | servo[it].target=900; |
matsu | 0:09bf8e3091e8 | 103 | servo[it].perTime=0; |
matsu | 0:09bf8e3091e8 | 104 | servo[it].moveFlag=false; |
matsu | 0:09bf8e3091e8 | 105 | servo[it].preTarget=900; |
matsu | 0:09bf8e3091e8 | 106 | servo[it].resolution=0; |
matsu | 0:09bf8e3091e8 | 107 | servo[it].positionNow=900; |
matsu | 0:09bf8e3091e8 | 108 | } |
matsu | 0:09bf8e3091e8 | 109 | |
matsu | 0:09bf8e3091e8 | 110 | myLed1=0; |
matsu | 0:09bf8e3091e8 | 111 | myLed2=0; |
matsu | 0:09bf8e3091e8 | 112 | myLed3=0; |
matsu | 0:09bf8e3091e8 | 113 | |
matsu | 0:09bf8e3091e8 | 114 | |
matsu | 0:09bf8e3091e8 | 115 | int valueNumber=0; |
matsu | 0:09bf8e3091e8 | 116 | int id=0; |
matsu | 0:09bf8e3091e8 | 117 | int command=0; |
matsu | 0:09bf8e3091e8 | 118 | int getValue=0; |
matsu | 0:09bf8e3091e8 | 119 | int checkSum=0; |
matsu | 0:09bf8e3091e8 | 120 | |
matsu | 0:09bf8e3091e8 | 121 | while(1){ |
matsu | 0:09bf8e3091e8 | 122 | if(device.readable()){ |
matsu | 0:09bf8e3091e8 | 123 | valueNumber++; |
matsu | 0:09bf8e3091e8 | 124 | getValue = device.getc(); |
matsu | 0:09bf8e3091e8 | 125 | |
matsu | 0:09bf8e3091e8 | 126 | switch(valueNumber){ |
matsu | 0:09bf8e3091e8 | 127 | case 1: //get ID |
matsu | 0:09bf8e3091e8 | 128 | id = getValue; |
matsu | 0:09bf8e3091e8 | 129 | checkSum = getValue; |
matsu | 0:09bf8e3091e8 | 130 | break; |
matsu | 0:09bf8e3091e8 | 131 | case 2: //get command upbyte |
matsu | 0:09bf8e3091e8 | 132 | command = getValue; |
matsu | 0:09bf8e3091e8 | 133 | checkSum += getValue; |
matsu | 0:09bf8e3091e8 | 134 | break; |
matsu | 0:09bf8e3091e8 | 135 | case 3: |
matsu | 0:09bf8e3091e8 | 136 | case 4: |
matsu | 0:09bf8e3091e8 | 137 | case 5: |
matsu | 0:09bf8e3091e8 | 138 | case 6: |
matsu | 0:09bf8e3091e8 | 139 | case 7: |
matsu | 0:09bf8e3091e8 | 140 | |
matsu | 0:09bf8e3091e8 | 141 | //servo_move_command |
matsu | 0:09bf8e3091e8 | 142 | if(command == SERVO_MOVE_COMMAND){ |
matsu | 0:09bf8e3091e8 | 143 | if((valueNumber == SERVO_MOVE_BYTE && (checkSum & 0x0FF) == getValue) || valueNumber < SERVO_MOVE_BYTE){ |
matsu | 0:09bf8e3091e8 | 144 | if(0 < id && id < 7){ |
matsu | 0:09bf8e3091e8 | 145 | servo[id].moveFlag = true; |
matsu | 0:09bf8e3091e8 | 146 | }else if(id == 0x0FE){ |
matsu | 0:09bf8e3091e8 | 147 | for(int it=1; it<7; it++){ |
matsu | 0:09bf8e3091e8 | 148 | servo[it].moveFlag = true; |
matsu | 0:09bf8e3091e8 | 149 | } |
matsu | 0:09bf8e3091e8 | 150 | } |
matsu | 0:09bf8e3091e8 | 151 | if(valueNumber == SERVO_MOVE_BYTE){ |
matsu | 0:09bf8e3091e8 | 152 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 153 | device.putc(ACK); //ACK |
matsu | 0:09bf8e3091e8 | 154 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 155 | } |
matsu | 0:09bf8e3091e8 | 156 | }else{ |
matsu | 0:09bf8e3091e8 | 157 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 158 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 159 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 160 | } |
matsu | 0:09bf8e3091e8 | 161 | } |
matsu | 0:09bf8e3091e8 | 162 | |
matsu | 0:09bf8e3091e8 | 163 | |
matsu | 0:09bf8e3091e8 | 164 | //senddata command check |
matsu | 0:09bf8e3091e8 | 165 | if(command == SENDDATA_COMMAND){ |
matsu | 0:09bf8e3091e8 | 166 | if((valueNumber == SENDDATA_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < SENDDATA_BYTE){ |
matsu | 0:09bf8e3091e8 | 167 | int send_data = 0; |
matsu | 0:09bf8e3091e8 | 168 | switch(id){ |
matsu | 0:09bf8e3091e8 | 169 | case 1: //x |
matsu | 0:09bf8e3091e8 | 170 | send_data = ain1.read_u16(); |
matsu | 0:09bf8e3091e8 | 171 | device.putc(send_data >> 8); |
matsu | 0:09bf8e3091e8 | 172 | device.putc(send_data & 0xFF); |
matsu | 0:09bf8e3091e8 | 173 | break; |
matsu | 0:09bf8e3091e8 | 174 | case 2: //y |
matsu | 0:09bf8e3091e8 | 175 | send_data = ain2.read_u16(); |
matsu | 0:09bf8e3091e8 | 176 | device.putc(send_data >> 8); |
matsu | 0:09bf8e3091e8 | 177 | device.putc(send_data & 0xFF); |
matsu | 0:09bf8e3091e8 | 178 | break; |
matsu | 0:09bf8e3091e8 | 179 | case 3: //z |
matsu | 0:09bf8e3091e8 | 180 | send_data = ain3.read_u16(); |
matsu | 0:09bf8e3091e8 | 181 | device.putc(send_data >> 8); |
matsu | 0:09bf8e3091e8 | 182 | device.putc(send_data & 0xFF); |
matsu | 0:09bf8e3091e8 | 183 | break; |
matsu | 0:09bf8e3091e8 | 184 | case 4: //other |
matsu | 0:09bf8e3091e8 | 185 | send_data = ain4.read_u16(); |
matsu | 0:09bf8e3091e8 | 186 | device.putc(send_data >> 8); |
matsu | 0:09bf8e3091e8 | 187 | device.putc(send_data & 0xFF); |
matsu | 0:09bf8e3091e8 | 188 | break; |
matsu | 0:09bf8e3091e8 | 189 | case 5: //other |
matsu | 0:09bf8e3091e8 | 190 | send_data = ain5.read_u16(); |
matsu | 0:09bf8e3091e8 | 191 | device.putc(send_data >> 8); |
matsu | 0:09bf8e3091e8 | 192 | device.putc(send_data & 0xFF); |
matsu | 0:09bf8e3091e8 | 193 | break; |
matsu | 0:09bf8e3091e8 | 194 | case 6: //other |
matsu | 0:09bf8e3091e8 | 195 | send_data = ain6.read_u16(); |
matsu | 0:09bf8e3091e8 | 196 | device.putc(send_data >> 8); |
matsu | 0:09bf8e3091e8 | 197 | device.putc(send_data & 0xFF); |
matsu | 0:09bf8e3091e8 | 198 | break; |
matsu | 0:09bf8e3091e8 | 199 | default: |
matsu | 0:09bf8e3091e8 | 200 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 201 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 202 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 203 | break; |
matsu | 0:09bf8e3091e8 | 204 | } |
matsu | 0:09bf8e3091e8 | 205 | if(valueNumber == SENDDATA_BYTE){ |
matsu | 0:09bf8e3091e8 | 206 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 207 | device.putc(ACK); //ACK |
matsu | 0:09bf8e3091e8 | 208 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 209 | } |
matsu | 0:09bf8e3091e8 | 210 | }else{ |
matsu | 0:09bf8e3091e8 | 211 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 212 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 213 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 214 | } |
matsu | 0:09bf8e3091e8 | 215 | } |
matsu | 0:09bf8e3091e8 | 216 | |
matsu | 0:09bf8e3091e8 | 217 | //power_onoff command check |
matsu | 0:09bf8e3091e8 | 218 | if(command == POWER_ONOFF_COMMAND){ |
matsu | 0:09bf8e3091e8 | 219 | if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){ |
matsu | 0:09bf8e3091e8 | 220 | switch(id){ |
matsu | 0:09bf8e3091e8 | 221 | case 1: |
matsu | 0:09bf8e3091e8 | 222 | servo1.onOff(); |
matsu | 0:09bf8e3091e8 | 223 | break; |
matsu | 0:09bf8e3091e8 | 224 | case 2: |
matsu | 0:09bf8e3091e8 | 225 | servo2.onOff(); |
matsu | 0:09bf8e3091e8 | 226 | break; |
matsu | 0:09bf8e3091e8 | 227 | case 3: |
matsu | 0:09bf8e3091e8 | 228 | servo3.onOff(); |
matsu | 0:09bf8e3091e8 | 229 | break; |
matsu | 0:09bf8e3091e8 | 230 | case 4: |
matsu | 0:09bf8e3091e8 | 231 | servo4.onOff(); |
matsu | 0:09bf8e3091e8 | 232 | break; |
matsu | 0:09bf8e3091e8 | 233 | case 5: |
matsu | 0:09bf8e3091e8 | 234 | servo5.onOff(); |
matsu | 0:09bf8e3091e8 | 235 | break; |
matsu | 0:09bf8e3091e8 | 236 | case 6: |
matsu | 0:09bf8e3091e8 | 237 | servo6.onOff(); |
matsu | 0:09bf8e3091e8 | 238 | break; |
matsu | 0:09bf8e3091e8 | 239 | default : |
matsu | 0:09bf8e3091e8 | 240 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 241 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 242 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 243 | break; |
matsu | 0:09bf8e3091e8 | 244 | } |
matsu | 0:09bf8e3091e8 | 245 | if(valueNumber == POWER_ONOFF_BYTE){ |
matsu | 0:09bf8e3091e8 | 246 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 247 | device.putc(ACK); //ACK |
matsu | 0:09bf8e3091e8 | 248 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 249 | } |
matsu | 0:09bf8e3091e8 | 250 | }else{ |
matsu | 0:09bf8e3091e8 | 251 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 252 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 253 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 254 | } |
matsu | 0:09bf8e3091e8 | 255 | } |
matsu | 0:09bf8e3091e8 | 256 | |
matsu | 0:09bf8e3091e8 | 257 | //init command check |
matsu | 0:09bf8e3091e8 | 258 | if(command == INIT_COMMAND){ |
matsu | 0:09bf8e3091e8 | 259 | if((valueNumber == INIT_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < INIT_BYTE){ |
matsu | 0:09bf8e3091e8 | 260 | switch(id){ |
matsu | 0:09bf8e3091e8 | 261 | case 1: |
matsu | 0:09bf8e3091e8 | 262 | servo1.homePosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 263 | break; |
matsu | 0:09bf8e3091e8 | 264 | case 2: |
matsu | 0:09bf8e3091e8 | 265 | servo2.homePosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 266 | break; |
matsu | 0:09bf8e3091e8 | 267 | case 3: |
matsu | 0:09bf8e3091e8 | 268 | servo3.homePosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 269 | break; |
matsu | 0:09bf8e3091e8 | 270 | case 4: |
matsu | 0:09bf8e3091e8 | 271 | servo4.homePosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 272 | break; |
matsu | 0:09bf8e3091e8 | 273 | case 5: |
matsu | 0:09bf8e3091e8 | 274 | servo5.homePosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 275 | break; |
matsu | 0:09bf8e3091e8 | 276 | case 6: |
matsu | 0:09bf8e3091e8 | 277 | servo6.homePosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 278 | break; |
matsu | 0:09bf8e3091e8 | 279 | default : |
matsu | 0:09bf8e3091e8 | 280 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 281 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 282 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 283 | break; |
matsu | 0:09bf8e3091e8 | 284 | } |
matsu | 0:09bf8e3091e8 | 285 | if(valueNumber == INIT_BYTE){ |
matsu | 0:09bf8e3091e8 | 286 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 287 | device.putc(ACK); //ACK |
matsu | 0:09bf8e3091e8 | 288 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 289 | } |
matsu | 0:09bf8e3091e8 | 290 | }else{ |
matsu | 0:09bf8e3091e8 | 291 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 292 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 293 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 294 | } |
matsu | 0:09bf8e3091e8 | 295 | } |
matsu | 0:09bf8e3091e8 | 296 | |
matsu | 0:09bf8e3091e8 | 297 | //move command check |
matsu | 0:09bf8e3091e8 | 298 | if(command == targetPosition_COMMAND || command == SET_targetPosition_COMMAND){ |
matsu | 0:09bf8e3091e8 | 299 | if((valueNumber == targetPosition_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < targetPosition_BYTE){ |
matsu | 0:09bf8e3091e8 | 300 | switch(id){ |
matsu | 0:09bf8e3091e8 | 301 | case 1: |
matsu | 0:09bf8e3091e8 | 302 | servo1.targetPosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 303 | servo[id].target = servo1.getPosition(); |
matsu | 0:09bf8e3091e8 | 304 | servo[id].perTime = servo1.getTime(); |
matsu | 0:09bf8e3091e8 | 305 | break; |
matsu | 0:09bf8e3091e8 | 306 | case 2: |
matsu | 0:09bf8e3091e8 | 307 | servo2.targetPosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 308 | servo[id].target = servo2.getPosition(); |
matsu | 0:09bf8e3091e8 | 309 | servo[id].perTime = servo2.getTime(); |
matsu | 0:09bf8e3091e8 | 310 | break; |
matsu | 0:09bf8e3091e8 | 311 | case 3: |
matsu | 0:09bf8e3091e8 | 312 | servo3.targetPosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 313 | servo[id].target = servo3.getPosition(); |
matsu | 0:09bf8e3091e8 | 314 | servo[id].perTime = servo3.getTime(); |
matsu | 0:09bf8e3091e8 | 315 | break; |
matsu | 0:09bf8e3091e8 | 316 | case 4: |
matsu | 0:09bf8e3091e8 | 317 | servo4.targetPosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 318 | servo[id].target = servo4.getPosition(); |
matsu | 0:09bf8e3091e8 | 319 | servo[id].perTime = servo4.getTime(); |
matsu | 0:09bf8e3091e8 | 320 | break; |
matsu | 0:09bf8e3091e8 | 321 | case 5: |
matsu | 0:09bf8e3091e8 | 322 | servo5.targetPosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 323 | servo[id].target = servo5.getPosition(); |
matsu | 0:09bf8e3091e8 | 324 | servo[id].perTime = servo5.getTime(); |
matsu | 0:09bf8e3091e8 | 325 | break; |
matsu | 0:09bf8e3091e8 | 326 | case 6: |
matsu | 0:09bf8e3091e8 | 327 | servo6.targetPosition(valueNumber,getValue); |
matsu | 0:09bf8e3091e8 | 328 | servo[id].target = servo6.getPosition(); |
matsu | 0:09bf8e3091e8 | 329 | servo[id].perTime = servo6.getTime(); |
matsu | 0:09bf8e3091e8 | 330 | break; |
matsu | 0:09bf8e3091e8 | 331 | } |
matsu | 0:09bf8e3091e8 | 332 | if(valueNumber == targetPosition_BYTE){ |
matsu | 0:09bf8e3091e8 | 333 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 334 | device.putc(ACK); //ACK |
matsu | 0:09bf8e3091e8 | 335 | servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00)); |
matsu | 0:09bf8e3091e8 | 336 | if(command == targetPosition_COMMAND){ |
matsu | 0:09bf8e3091e8 | 337 | servo[id].moveFlag = true; |
matsu | 0:09bf8e3091e8 | 338 | if(id == 1){ |
matsu | 0:09bf8e3091e8 | 339 | myLed1 = 1; |
matsu | 0:09bf8e3091e8 | 340 | }else if(id == 2){ |
matsu | 0:09bf8e3091e8 | 341 | myLed2 = 2; |
matsu | 0:09bf8e3091e8 | 342 | }else if(id == 3){ |
matsu | 0:09bf8e3091e8 | 343 | myLed3 = 3; |
matsu | 0:09bf8e3091e8 | 344 | } |
matsu | 0:09bf8e3091e8 | 345 | }else if(command == SET_targetPosition_COMMAND){ |
matsu | 0:09bf8e3091e8 | 346 | } |
matsu | 0:09bf8e3091e8 | 347 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 348 | } |
matsu | 0:09bf8e3091e8 | 349 | }else{ |
matsu | 0:09bf8e3091e8 | 350 | device.putc(id); |
matsu | 0:09bf8e3091e8 | 351 | device.putc(NACK); //NACK |
matsu | 0:09bf8e3091e8 | 352 | valueNumber = 0; |
matsu | 0:09bf8e3091e8 | 353 | } |
matsu | 0:09bf8e3091e8 | 354 | } |
matsu | 0:09bf8e3091e8 | 355 | //plus checkSum |
matsu | 0:09bf8e3091e8 | 356 | checkSum += getValue; |
matsu | 0:09bf8e3091e8 | 357 | break; |
matsu | 0:09bf8e3091e8 | 358 | |
matsu | 0:09bf8e3091e8 | 359 | } |
matsu | 0:09bf8e3091e8 | 360 | } |
matsu | 0:09bf8e3091e8 | 361 | } |
matsu | 0:09bf8e3091e8 | 362 | } |
matsu | 0:09bf8e3091e8 | 363 |