main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "RS405cb.h"
00004 
00005 #define PI 3.141592653589793
00006 
00007 Serial pc(USBTX, USBRX);
00008 DigitalOut debug1(LED1);
00009 DigitalOut debug2(LED2);
00010 DigitalOut debug3(LED3);
00011 DigitalOut debug4(LED4);
00012 RS405cb servo(p13,p14,p15);
00013 
00014 double Theta1 = 128, Theta2 = 128, Theta = 128;
00015 
00016 void setup(){
00017     servo.TORQUE_ON(1);
00018     servo.TORQUE_ON(2);
00019     servo.TORQUE_ON(3);
00020     servo.Rotate_Servo_Float(1,0.0);
00021     servo.Rotate_Servo_Float(2,0.0);
00022     servo.Rotate_Servo_Float(3,0.0);
00023     pc.format(8,Serial::None,1);
00024 }
00025 void izumi(){
00026     //0~255 to -180~180
00027     Theta1 = Theta1 * 360.0 / 255.0 - 180.0;
00028     Theta2 = Theta2 * 360.0 / 255.0 - 180.0;
00029     Theta = (Theta * 360.0 / 255.0 - 180.0) / 2;    //gear 1:2
00030 }    
00031 void robot_arm(int theta_1, int theta_2,int theta,int theta0_1,int theta0_2 , int theta0) {
00032     //theta_1 : 1 ban ue no arm
00033     //theta_2 : mannnaka no arm
00034     //theta   : 1 ban sita no arm
00035         double m1,m2,m3;
00036     for(int i=0; i<=120; i++) {
00037         m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
00038         servo.Rotate_Servo_Float(3,m3);       
00039         wait(0.01);
00040     }
00041     for(int i=0; i<=120; i++) {
00042         m1 = -theta0_1-(theta_1-theta0_1)/2.0*(1-cos(PI*i/120.0));
00043         m2 = theta0_2+(theta_2-theta0_2)/2.0*(1-cos(PI*i/120.0));
00044         servo.Rotate_Servo_Float(1,m1);
00045         servo.Rotate_Servo_Float(2,m2);             
00046         wait(0.01);
00047     }
00048 }
00049 
00050 int main() {
00051     setup();
00052     
00053     debug1 = 1;
00054     pc.putc(0);         //Image analysis order
00055     //wait(7.0);          //Waiting analysis. 
00056     int Wait = pc.getc();
00057     debug1 = 1;
00058     pc.putc(1);         //Theta get order
00059     Theta1 = pc.getc();   //Get Theta1
00060     debug2 = 1;
00061     
00062     pc.putc(2);         //Theta2 get order
00063     Theta2 = pc.getc();   //Get Theta2
00064     debug3 = 1;
00065     
00066     pc.putc(3);         //Theta1 get order
00067     Theta = pc.getc();   //Get Theta
00068     debug4 = 1;
00069 
00070     izumi();
00071     while(1){
00072         robot_arm(Theta1, Theta2, Theta,0,0,0);
00073         wait(2.0);
00074         robot_arm(0,0,0,Theta1,Theta2,Theta);
00075         wait(2.0);
00076     }
00077 }