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