Hiroaki Matsuda
/
mbed-for-McuComm
this is program for mbed ,to relay MCUcomm data to servo
main.cpp
- Committer:
- matsu
- Date:
- 2012-12-05
- Revision:
- 1:0fe52e356d01
- Parent:
- 0:09bf8e3091e8
File content as of revision 1:0fe52e356d01:
#include "mbed.h" #include "Servo.h" #include "command.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_ON_COMMAND 0x59 #define POWER_OFF_COMMAND 0x5A #define SENDDATA_COMMAND 0x05 #define INIT_COMMAND 0x66 #define TARGETPOSITION_BYTE 7 #define INIT_BYTE 6 #define SERVO_MOVE_BYTE 3 #define POWER_ONOFF_BYTE 3 #define SENDDATA_BYTE 3 #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); I2C i2c(p9,p10); typedef struct{ short int target; short int preTarget; long perTime; bool moveFlag; int resolution; int 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=(double)servo[it].positionNow/MAX_DATA; break; case 2: servo2=(double)servo[it].positionNow/MAX_DATA; break; case 3: servo3=(double)servo[it].positionNow/MAX_DATA; break; case 4: servo4=(double)servo[it].positionNow/MAX_DATA; break; case 5: servo5=(double)servo[it].positionNow/MAX_DATA; break; case 6: servo6=(double)servo[it].positionNow/MAX_DATA; break; } } } } int main() { device.baud(115200); flipper.attach_us(&flip, TIME_INTERRAPUT_US); // i2c.frequency(40000); char cmd[6]; char add[1]; char init[2]; add[0] = 0x01; init[0] = 0x2A; init[1] = 0x01; // while(i2c.write(0x3A,init,2)); 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){ if(servo[id].moveFlag == true) servo[id].preTarget = servo[id].positionNow; servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00000)); if(servo[id].resolution != 0){ servo[id].moveFlag = true; if(id == 1) myLed1 = 1; else if(id == 2) myLed2 = 2; else if(id == 3) myLed3 = 3; } }else if(id == 0x0FE){ for(int it=1; it<7; it++){ if(servo[it].resolution != 0){ servo[it].moveFlag = true; if(it == 1) myLed1 = 1; else if(it == 2) myLed2 = 2; else if(it == 3) myLed3 = 3; } } } 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){ myLed1 = 1; /* i2c.write(0x3A,add,1); i2c.read(0x3A,cmd,6); */ int send_data; switch(id){ case 1: //x // myLed1 = 1; 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(0); device.putc(0); device.putc(id); device.putc(NACK); //NACK valueNumber = 0; break; } if(valueNumber == SENDDATA_BYTE && id<=6){ device.putc(id); device.putc(ACK); //ACK valueNumber = 0; } }else{ device.putc(0); device.putc(0); device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //power_onoff command check if(command == POWER_ON_COMMAND){ if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){ switch(id){ case 1: servo1.on(); break; case 2: servo2.on(); break; case 3: servo3.on(); break; case 4: servo4.on(); break; case 5: servo5.on(); break; case 6: servo6.on(); 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; } } //power_onoff command check if(command == POWER_OFF_COMMAND){ if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){ switch(id){ case 1: servo1.off(); break; case 2: servo2.off(); break; case 3: servo3.off(); break; case 4: servo4.off(); break; case 5: servo5.off(); break; case 6: servo6.off(); 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 if(command == TARGETPOSITION_COMMAND){ if(servo[id].moveFlag == true) servo[id].preTarget = servo[id].positionNow; servo[id].resolution = (servo[id].target-servo[id].preTarget)/(servo[id].perTime/(TIME_INTERRAPUT_US/1000.00000)); if(servo[id].resolution != 0){ servo[id].moveFlag = true; if(id == 1) myLed1 = 1; else if(id == 2) myLed2 = 2; else if(id == 3) myLed3 = 3; } } valueNumber = 0; } }else{ device.putc(id); device.putc(NACK); //NACK valueNumber = 0; } } //plus checkSum checkSum += getValue; break; } } } }