Hiroaki Matsuda
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;
+
+ }
+ }
+ }
+}
+