YUICHIRO IZUMI / Mbed 2 deprecated ohta

aaa

Dependencies:   RS405cb enc mbed

main.cpp@0:b90181e726c7, 2014-01-29 (annotated)

Committer:
yizumi1012xxx
Date:
Wed Jan 29 09:46:41 2014 +0000
Revision:
0:b90181e726c7
Child:
1:f4719bf17974
for ohta

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yizumi1012xxx 0:b90181e726c7 1 #include "mbed.h"
yizumi1012xxx 0:b90181e726c7 2 #include "QEI.h"
yizumi1012xxx 0:b90181e726c7 3 #include "RS405cb.h"
yizumi1012xxx 0:b90181e726c7 4
yizumi1012xxx 0:b90181e726c7 5 #define PI 3.141592653589793
yizumi1012xxx 0:b90181e726c7 6
yizumi1012xxx 0:b90181e726c7 7 Serial pc(USBTX, USBRX);
yizumi1012xxx 0:b90181e726c7 8 DigitalOut debug1(LED1);
yizumi1012xxx 0:b90181e726c7 9 DigitalOut debug2(LED2);
yizumi1012xxx 0:b90181e726c7 10 DigitalOut debug3(LED3);
yizumi1012xxx 0:b90181e726c7 11 DigitalOut debug4(LED4);
yizumi1012xxx 0:b90181e726c7 12 RS405cb servo(p13,p14,p15);
yizumi1012xxx 0:b90181e726c7 13
yizumi1012xxx 0:b90181e726c7 14 double Theta1 = 128, Theta2 = 128, Theta = 128;
yizumi1012xxx 0:b90181e726c7 15
yizumi1012xxx 0:b90181e726c7 16 void setup(){
yizumi1012xxx 0:b90181e726c7 17 servo.TORQUE_ON(1);
yizumi1012xxx 0:b90181e726c7 18 servo.TORQUE_ON(2);
yizumi1012xxx 0:b90181e726c7 19 servo.TORQUE_ON(3);
yizumi1012xxx 0:b90181e726c7 20 servo.Rotate_Servo_Float(1,0.0);
yizumi1012xxx 0:b90181e726c7 21 servo.Rotate_Servo_Float(2,0.0);
yizumi1012xxx 0:b90181e726c7 22 servo.Rotate_Servo_Float(3,0.0);
yizumi1012xxx 0:b90181e726c7 23 pc.format(8,Serial::None,1);
yizumi1012xxx 0:b90181e726c7 24 }
yizumi1012xxx 0:b90181e726c7 25 void izumi(){
yizumi1012xxx 0:b90181e726c7 26 //0~255 to -180~180
yizumi1012xxx 0:b90181e726c7 27 Theta1 = Theta1 * 360.0 / 255.0 - 180.0;
yizumi1012xxx 0:b90181e726c7 28 Theta2 = Theta2 * 360.0 / 255.0 - 180.0;
yizumi1012xxx 0:b90181e726c7 29 Theta = (Theta * 360.0 / 255.0 - 180.0) / 2; //gear 1:2
yizumi1012xxx 0:b90181e726c7 30 }
yizumi1012xxx 0:b90181e726c7 31 void robot_arm(int theta_1, int theta_2,int theta,int theta0_1,int theta0_2 , int theta0) {
yizumi1012xxx 0:b90181e726c7 32 //theta_1 : 1 ban ue no arm
yizumi1012xxx 0:b90181e726c7 33 //theta_2 : mannnaka no arm
yizumi1012xxx 0:b90181e726c7 34 //theta : 1 ban sita no arm
yizumi1012xxx 0:b90181e726c7 35 for(int i=0; i<=120; i++) {
yizumi1012xxx 0:b90181e726c7 36 double m1,m2,m3;
yizumi1012xxx 0:b90181e726c7 37 m1 = -theta0_1-(theta_1-theta0_1)/2.0*(1-cos(PI*i/120.0));
yizumi1012xxx 0:b90181e726c7 38 m2 = theta0_2+(theta_2-theta0_2)/2.0*(1-cos(PI*i/120.0));
yizumi1012xxx 0:b90181e726c7 39 m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
yizumi1012xxx 0:b90181e726c7 40 servo.Rotate_Servo_Float(1,m1);
yizumi1012xxx 0:b90181e726c7 41 servo.Rotate_Servo_Float(2,m2);
yizumi1012xxx 0:b90181e726c7 42 servo.Rotate_Servo_Float(3,m3);
yizumi1012xxx 0:b90181e726c7 43 wait(0.01);
yizumi1012xxx 0:b90181e726c7 44 }
yizumi1012xxx 0:b90181e726c7 45 }
yizumi1012xxx 0:b90181e726c7 46
yizumi1012xxx 0:b90181e726c7 47 int main() {
yizumi1012xxx 0:b90181e726c7 48 setup();
yizumi1012xxx 0:b90181e726c7 49
yizumi1012xxx 0:b90181e726c7 50 pc.putc(0); //Image analysis order
yizumi1012xxx 0:b90181e726c7 51 //wait(7.0); //Waiting analysis.
yizumi1012xxx 0:b90181e726c7 52 int Wait = pc.getc();
yizumi1012xxx 0:b90181e726c7 53 debug1 = 1;
yizumi1012xxx 0:b90181e726c7 54 pc.putc(1); //Theta get order
yizumi1012xxx 0:b90181e726c7 55 Theta1 = pc.getc(); //Get Theta1
yizumi1012xxx 0:b90181e726c7 56 debug2 = 1;
yizumi1012xxx 0:b90181e726c7 57
yizumi1012xxx 0:b90181e726c7 58 pc.putc(2); //Theta2 get order
yizumi1012xxx 0:b90181e726c7 59 Theta2 = pc.getc(); //Get Theta2
yizumi1012xxx 0:b90181e726c7 60 debug3 = 1;
yizumi1012xxx 0:b90181e726c7 61
yizumi1012xxx 0:b90181e726c7 62 pc.putc(3); //Theta1 get order
yizumi1012xxx 0:b90181e726c7 63 Theta = pc.getc(); //Get Theta
yizumi1012xxx 0:b90181e726c7 64 debug4 = 1;
yizumi1012xxx 0:b90181e726c7 65
yizumi1012xxx 0:b90181e726c7 66 izumi();
yizumi1012xxx 0:b90181e726c7 67 while(1){
yizumi1012xxx 0:b90181e726c7 68 robot_arm(Theta1, Theta2, Theta,0,0,0);
yizumi1012xxx 0:b90181e726c7 69 wait(2.0);
yizumi1012xxx 0:b90181e726c7 70 robot_arm(0,0,0,Theta1,Theta2,Theta);
yizumi1012xxx 0:b90181e726c7 71 wait(2.0);
yizumi1012xxx 0:b90181e726c7 72 }
yizumi1012xxx 0:b90181e726c7 73 }