YUICHIRO IZUMI / Mbed 2 deprecated ohta

aaa

Dependencies:   RS405cb enc mbed

main.cpp@1:f4719bf17974, 2014-01-30 (annotated)

Committer:
yizumi1012xxx
Date:
Thu Jan 30 04:56:41 2014 +0000
Revision:
1:f4719bf17974
Parent:
0:b90181e726c7 
forayami;

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 1:f4719bf17974 35 double m1,m2,m3;
yizumi1012xxx 0:b90181e726c7 36 for(int i=0; i<=120; i++) {
yizumi1012xxx 1:f4719bf17974 37 m3 = theta0+(theta-theta0)/2.0*(1-cos(PI*i/120.0));
yizumi1012xxx 1:f4719bf17974 38 servo.Rotate_Servo_Float(3,m3);
yizumi1012xxx 1:f4719bf17974 39 wait(0.01);
yizumi1012xxx 1:f4719bf17974 40 }
yizumi1012xxx 1:f4719bf17974 41 for(int i=0; i<=120; i++) {
yizumi1012xxx 0:b90181e726c7 42 m1 = -theta0_1-(theta_1-theta0_1)/2.0*(1-cos(PI*i/120.0));
yizumi1012xxx 0:b90181e726c7 43 m2 = theta0_2+(theta_2-theta0_2)/2.0*(1-cos(PI*i/120.0));
yizumi1012xxx 0:b90181e726c7 44 servo.Rotate_Servo_Float(1,m1);
yizumi1012xxx 0:b90181e726c7 45 servo.Rotate_Servo_Float(2,m2);
yizumi1012xxx 0:b90181e726c7 46 wait(0.01);
yizumi1012xxx 0:b90181e726c7 47 }
yizumi1012xxx 0:b90181e726c7 48 }
yizumi1012xxx 0:b90181e726c7 49
yizumi1012xxx 0:b90181e726c7 50 int main() {
yizumi1012xxx 0:b90181e726c7 51 setup();
yizumi1012xxx 0:b90181e726c7 52
yizumi1012xxx 1:f4719bf17974 53 debug1 = 1;
yizumi1012xxx 0:b90181e726c7 54 pc.putc(0); //Image analysis order
yizumi1012xxx 0:b90181e726c7 55 //wait(7.0); //Waiting analysis.
yizumi1012xxx 0:b90181e726c7 56 int Wait = pc.getc();
yizumi1012xxx 0:b90181e726c7 57 debug1 = 1;
yizumi1012xxx 0:b90181e726c7 58 pc.putc(1); //Theta get order
yizumi1012xxx 0:b90181e726c7 59 Theta1 = pc.getc(); //Get Theta1
yizumi1012xxx 0:b90181e726c7 60 debug2 = 1;
yizumi1012xxx 0:b90181e726c7 61
yizumi1012xxx 0:b90181e726c7 62 pc.putc(2); //Theta2 get order
yizumi1012xxx 0:b90181e726c7 63 Theta2 = pc.getc(); //Get Theta2
yizumi1012xxx 0:b90181e726c7 64 debug3 = 1;
yizumi1012xxx 0:b90181e726c7 65
yizumi1012xxx 0:b90181e726c7 66 pc.putc(3); //Theta1 get order
yizumi1012xxx 0:b90181e726c7 67 Theta = pc.getc(); //Get Theta
yizumi1012xxx 0:b90181e726c7 68 debug4 = 1;
yizumi1012xxx 0:b90181e726c7 69
yizumi1012xxx 0:b90181e726c7 70 izumi();
yizumi1012xxx 0:b90181e726c7 71 while(1){
yizumi1012xxx 0:b90181e726c7 72 robot_arm(Theta1, Theta2, Theta,0,0,0);
yizumi1012xxx 0:b90181e726c7 73 wait(2.0);
yizumi1012xxx 0:b90181e726c7 74 robot_arm(0,0,0,Theta1,Theta2,Theta);
yizumi1012xxx 0:b90181e726c7 75 wait(2.0);
yizumi1012xxx 0:b90181e726c7 76 }
yizumi1012xxx 0:b90181e726c7 77 }