Hiroaki Matsuda
/
mbed-for-McuComm
this is program for mbed ,to relay MCUcomm data to servo
main.cpp
- Committer:
- matsu
- Date:
- 2012-11-28
- Revision:
- 0:09bf8e3091e8
- Child:
- 1:0fe52e356d01
File content as of revision 0:09bf8e3091e8:
#include "mbed.h" #include "Servo.h" #include "command.h" #include "iSerial.h" #define MAX_DATA 1800 #define TIME_INTERRAPUT_US 16000 #define targetPosition_COMMAND 0x64 #define SET_targetPosition_COMMAND 0x67 #define SERVO_MOVE_COMMAND 0x58 #define POWER_ONOFF_COMMAND 0x59 #define SENDDATA_COMMAND 0x05 #define INIT_COMMAND 0x66 #define targetPosition_BYTE 7 #define SERVO_MOVE_BYTE 3 #define POWER_ONOFF_BYTE 3 #define SENDDATA_BYTE 3 #define INIT_BYTE 6 #define ACK 0x06 #define NACK 0x15 Ticker flipper; Serial device(USBTX, USBRX); // tx, rx DigitalOut myLed1(LED1); DigitalOut myLed2(LED2); DigitalOut myLed3(LED3); Command servo1(p21); Command servo2(p22); Command servo3(p23); Command servo4(p24); Command servo5(p25); Command servo6(p26); AnalogIn ain1(p15); AnalogIn ain2(p16); AnalogIn ain3(p17); AnalogIn ain4(p18); AnalogIn ain5(p19); AnalogIn ain6(p20); typedef struct{ short int target; short int preTarget; long perTime; bool moveFlag; int resolution; double positionNow; }servo_t; servo_t servo[7]; void flip() { for(int it=1;it<7;it++){ if(servo[it].moveFlag==true){ servo[it].positionNow+=servo[it].resolution; 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)){ servo[it].moveFlag=false; servo[it].preTarget = servo[it].target; servo[it].positionNow = servo[it].target; if(it == 1){ myLed1 = 0; }else if(it == 2){ myLed2 = 0; }else if(it == 3){ myLed3 = 0; } } switch(it){ case 1: servo1=servo[it].positionNow/MAX_DATA; break; case 2: servo2=servo[it].positionNow/MAX_DATA; break; case 3: servo3=servo[it].positionNow/MAX_DATA; break; case 4: servo4=servo[it].positionNow/MAX_DATA; break; case 5: servo5=servo[it].positionNow/MAX_DATA; break; case 6: servo6=servo[it].positionNow/MAX_DATA; break; } } } } int main() { device.baud(115200); flipper.attach_us(&flip, TIME_INTERRAPUT_US); // flag=false; for(int it=0;it<7;it++){ servo[it].target=900; servo[it].perTime=0; servo[it].moveFlag=false; servo[it].preTarget=900; servo[it].resolution=0; servo[it].positionNow=900; } myLed1=0; myLed2=0; myLed3=0; int valueNumber=0; int id=0; int command=0; int getValue=0; int checkSum=0; while(1){ if(device.readable()){ valueNumber++; getValue = device.getc(); switch(valueNumber){ case 1: //get ID id = getValue; checkSum = getValue; break; case 2: //get command upbyte command = getValue; checkSum += getValue; break; case 3: case 4: case 5: case 6: case 7: //servo_move_command if(command == SERVO_MOVE_COMMAND){ if((valueNumber == SERVO_MOVE_BYTE && (checkSum & 0x0FF) == getValue) || valueNumber < SERVO_MOVE_BYTE){ if(0 < id && id < 7){ servo[id].moveFlag = true; }else if(id == 0x0FE){ for(int it=1; it<7; it++){ servo[it].moveFlag = true; } } if(valueNumber == SERVO_MOVE_BYTE){ device.putc(id); device.putc(ACK); //ACK valueNumber = 0; } }else{ device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //senddata command check if(command == SENDDATA_COMMAND){ if((valueNumber == SENDDATA_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < SENDDATA_BYTE){ int send_data = 0; switch(id){ case 1: //x send_data = ain1.read_u16(); device.putc(send_data >> 8); device.putc(send_data & 0xFF); break; case 2: //y send_data = ain2.read_u16(); device.putc(send_data >> 8); device.putc(send_data & 0xFF); break; case 3: //z send_data = ain3.read_u16(); device.putc(send_data >> 8); device.putc(send_data & 0xFF); break; case 4: //other send_data = ain4.read_u16(); device.putc(send_data >> 8); device.putc(send_data & 0xFF); break; case 5: //other send_data = ain5.read_u16(); device.putc(send_data >> 8); device.putc(send_data & 0xFF); break; case 6: //other send_data = ain6.read_u16(); device.putc(send_data >> 8); device.putc(send_data & 0xFF); break; default: device.putc(id); device.putc(NACK); //NACK valueNumber = 0; break; } if(valueNumber == SENDDATA_BYTE){ device.putc(id); device.putc(ACK); //ACK valueNumber = 0; } }else{ device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //power_onoff command check if(command == POWER_ONOFF_COMMAND){ if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){ switch(id){ case 1: servo1.onOff(); break; case 2: servo2.onOff(); break; case 3: servo3.onOff(); break; case 4: servo4.onOff(); break; case 5: servo5.onOff(); break; case 6: servo6.onOff(); break; default : device.putc(id); device.putc(NACK); //NACK valueNumber = 0; break; } if(valueNumber == POWER_ONOFF_BYTE){ device.putc(id); device.putc(ACK); //ACK valueNumber = 0; } }else{ device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //init command check if(command == INIT_COMMAND){ if((valueNumber == INIT_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < INIT_BYTE){ switch(id){ case 1: servo1.homePosition(valueNumber,getValue); break; case 2: servo2.homePosition(valueNumber,getValue); break; case 3: servo3.homePosition(valueNumber,getValue); break; case 4: servo4.homePosition(valueNumber,getValue); break; case 5: servo5.homePosition(valueNumber,getValue); break; case 6: servo6.homePosition(valueNumber,getValue); break; default : device.putc(id); device.putc(NACK); //NACK valueNumber = 0; break; } if(valueNumber == INIT_BYTE){ device.putc(id); device.putc(ACK); //ACK valueNumber = 0; } }else{ device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //move command check if(command == targetPosition_COMMAND || command == SET_targetPosition_COMMAND){ if((valueNumber == targetPosition_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < targetPosition_BYTE){ switch(id){ case 1: servo1.targetPosition(valueNumber,getValue); servo[id].target = servo1.getPosition(); servo[id].perTime = servo1.getTime(); break; case 2: servo2.targetPosition(valueNumber,getValue); servo[id].target = servo2.getPosition(); servo[id].perTime = servo2.getTime(); break; case 3: servo3.targetPosition(valueNumber,getValue); servo[id].target = servo3.getPosition(); servo[id].perTime = servo3.getTime(); break; case 4: servo4.targetPosition(valueNumber,getValue); servo[id].target = servo4.getPosition(); servo[id].perTime = servo4.getTime(); break; case 5: servo5.targetPosition(valueNumber,getValue); servo[id].target = servo5.getPosition(); servo[id].perTime = servo5.getTime(); break; case 6: servo6.targetPosition(valueNumber,getValue); servo[id].target = servo6.getPosition(); servo[id].perTime = servo6.getTime(); break; } if(valueNumber == targetPosition_BYTE){ device.putc(id); device.putc(ACK); //ACK servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00)); if(command == targetPosition_COMMAND){ servo[id].moveFlag = true; if(id == 1){ myLed1 = 1; }else if(id == 2){ myLed2 = 2; }else if(id == 3){ myLed3 = 3; } }else if(command == SET_targetPosition_COMMAND){ } valueNumber = 0; } }else{ device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //plus checkSum checkSum += getValue; break; } } } }