aaa
Dependencies: RS405cb enc mbed
main.cpp
- Committer:
- yizumi1012xxx
- Date:
- 2014-01-30
- Revision:
- 1:f4719bf17974
- Parent:
- 0:b90181e726c7
File content as of revision 1:f4719bf17974:
#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 double m1,m2,m3; for(int i=0; i<=120; i++) { m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0)); servo.Rotate_Servo_Float(3,m3); wait(0.01); } for(int i=0; i<=120; i++) { 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)); servo.Rotate_Servo_Float(1,m1); servo.Rotate_Servo_Float(2,m2); wait(0.01); } } int main() { setup(); debug1 = 1; 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); } }