Diff: main.cpp

Revision:

0:b90181e726c7

Child:

1:f4719bf17974

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 29 09:46:41 2014 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "RS405cb.h"
+
+#define PI 3.141592653589793
+
+Serial pc(USBTX, USBRX);
+DigitalOut debug1(LED1);
+DigitalOut debug2(LED2);
+DigitalOut debug3(LED3);
+DigitalOut debug4(LED4);
+RS405cb servo(p13,p14,p15);
+
+double Theta1 = 128, Theta2 = 128, Theta = 128;
+
+void setup(){
+    servo.TORQUE_ON(1);
+    servo.TORQUE_ON(2);
+    servo.TORQUE_ON(3);
+    servo.Rotate_Servo_Float(1,0.0);
+    servo.Rotate_Servo_Float(2,0.0);
+    servo.Rotate_Servo_Float(3,0.0);
+    pc.format(8,Serial::None,1);
+}
+void izumi(){
+    //0~255 to -180~180
+    Theta1 = Theta1 * 360.0 / 255.0 - 180.0;
+    Theta2 = Theta2 * 360.0 / 255.0 - 180.0;
+    Theta = (Theta * 360.0 / 255.0 - 180.0) / 2;    //gear 1:2
+}    
+void robot_arm(int theta_1, int theta_2,int theta,int theta0_1,int theta0_2 , int theta0) {
+    //theta_1 : 1 ban ue no arm
+    //theta_2 : mannnaka no arm
+    //theta   : 1 ban sita no arm
+    for(int i=0; i<=120; i++) {
+        double m1,m2,m3;
+        m1 = -theta0_1-(theta_1-theta0_1)/2.0*(1-cos(PI*i/120.0));
+        m2 = theta0_2+(theta_2-theta0_2)/2.0*(1-cos(PI*i/120.0));
+        m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
+        servo.Rotate_Servo_Float(1,m1);
+        servo.Rotate_Servo_Float(2,m2);             
+        servo.Rotate_Servo_Float(3,m3);       
+        wait(0.01);
+    }
+}
+
+int main() {
+    setup();
+    
+    pc.putc(0);         //Image analysis order
+    //wait(7.0);          //Waiting analysis. 
+    int Wait = pc.getc();
+    debug1 = 1;
+    pc.putc(1);         //Theta get order
+    Theta1 = pc.getc();   //Get Theta1
+    debug2 = 1;
+    
+    pc.putc(2);         //Theta2 get order
+    Theta2 = pc.getc();   //Get Theta2
+    debug3 = 1;
+    
+    pc.putc(3);         //Theta1 get order
+    Theta = pc.getc();   //Get Theta
+    debug4 = 1;
+
+    izumi();
+    while(1){
+        robot_arm(Theta1, Theta2, Theta,0,0,0);
+        wait(2.0);
+        robot_arm(0,0,0,Theta1,Theta2,Theta);
+        wait(2.0);
+    }
+}
\ No newline at end of file