Hiroaki Matsuda
this is program for mbed ,to relay MCUcomm data to servo
Diff: main.cpp
- Revision:
- 1:0fe52e356d01
- Parent:
- 0:09bf8e3091e8
--- a/main.cpp Wed Nov 28 17:12:00 2012 +0000
+++ b/main.cpp Wed Dec 05 15:13:57 2012 +0000
@@ -1,23 +1,23 @@
#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 TARGETPOSITION_COMMAND 0x64
+#define SET_TARGETPOSITION_COMMAND 0x67
#define SERVO_MOVE_COMMAND 0x58
-#define POWER_ONOFF_COMMAND 0x59
+#define POWER_ON_COMMAND 0x59
+#define POWER_OFF_COMMAND 0x5A
#define SENDDATA_COMMAND 0x05
#define INIT_COMMAND 0x66
-#define targetPosition_BYTE 7
+#define TARGETPOSITION_BYTE 7
+#define INIT_BYTE 6
#define SERVO_MOVE_BYTE 3
#define POWER_ONOFF_BYTE 3
#define SENDDATA_BYTE 3
-#define INIT_BYTE 6
#define ACK 0x06
#define NACK 0x15
@@ -42,13 +42,15 @@
AnalogIn ain5(p19);
AnalogIn ain6(p20);
+I2C i2c(p9,p10);
+
typedef struct{
short int target;
short int preTarget;
long perTime;
bool moveFlag;
int resolution;
- double positionNow;
+ int positionNow;
}servo_t;
servo_t servo[7];
@@ -56,7 +58,7 @@
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)){
+ 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;
@@ -70,22 +72,22 @@
}
switch(it){
case 1:
- servo1=servo[it].positionNow/MAX_DATA;
+ servo1=(double)servo[it].positionNow/MAX_DATA;
break;
case 2:
- servo2=servo[it].positionNow/MAX_DATA;
+ servo2=(double)servo[it].positionNow/MAX_DATA;
break;
case 3:
- servo3=servo[it].positionNow/MAX_DATA;
+ servo3=(double)servo[it].positionNow/MAX_DATA;
break;
case 4:
- servo4=servo[it].positionNow/MAX_DATA;
+ servo4=(double)servo[it].positionNow/MAX_DATA;
break;
case 5:
- servo5=servo[it].positionNow/MAX_DATA;
+ servo5=(double)servo[it].positionNow/MAX_DATA;
break;
case 6:
- servo6=servo[it].positionNow/MAX_DATA;
+ servo6=(double)servo[it].positionNow/MAX_DATA;
break;
}
}
@@ -96,8 +98,15 @@
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));
-// flag=false;
for(int it=0;it<7;it++){
servo[it].target=900;
servo[it].perTime=0;
@@ -110,19 +119,16 @@
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();
-
+ getValue = device.getc();
switch(valueNumber){
case 1: //get ID
id = getValue;
@@ -137,15 +143,33 @@
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;
+ 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++){
- servo[it].moveFlag = true;
+ 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){
@@ -164,9 +188,16 @@
//senddata command check
if(command == SENDDATA_COMMAND){
if((valueNumber == SENDDATA_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < SENDDATA_BYTE){
- int send_data = 0;
+ 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);
@@ -197,12 +228,57 @@
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){
+
+ 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;
@@ -212,29 +288,29 @@
device.putc(NACK); //NACK
valueNumber = 0;
}
- }
-
+ }
+
//power_onoff command check
- if(command == POWER_ONOFF_COMMAND){
+ if(command == POWER_OFF_COMMAND){
if((valueNumber == POWER_ONOFF_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < POWER_ONOFF_BYTE){
switch(id){
case 1:
- servo1.onOff();
+ servo1.off();
break;
case 2:
- servo2.onOff();
+ servo2.off();
break;
case 3:
- servo3.onOff();
+ servo3.off();
break;
case 4:
- servo4.onOff();
+ servo4.off();
break;
case 5:
- servo5.onOff();
+ servo5.off();
break;
case 6:
- servo6.onOff();
+ servo6.off();
break;
default :
device.putc(id);
@@ -295,8 +371,8 @@
}
//move command check
- if(command == targetPosition_COMMAND || command == SET_targetPosition_COMMAND){
- if((valueNumber == targetPosition_BYTE && (checkSum & 0xFF) == getValue) || valueNumber < targetPosition_BYTE){
+ 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);
@@ -329,20 +405,23 @@
servo[id].perTime = servo6.getTime();
break;
}
- if(valueNumber == targetPosition_BYTE){
+ 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){
+
+ 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;
}