Hiroaki Matsuda
/
mbed-for-McuComm
this is program for mbed ,to relay MCUcomm data to servo
Diff: main.cpp
- Revision:
- 0:09bf8e3091e8
- Child:
- 1:0fe52e356d01
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Nov 28 17:12:00 2012 +0000 @@ -0,0 +1,363 @@ +#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; + + } + } + } +} +