Kei Ohta / Mbed 2 deprecated ToyBox

for izumi and ayami

Dependencies:   RS405cb enc mbed

main.cpp@2:531e6b0346e6, 2013-12-12 (annotated)

Committer:
ohtake_i
Date:
Thu Dec 12 15:01:32 2013 +0000
Revision:
2:531e6b0346e6
Parent:
1:2fddb5d4ad27 
for ayami;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ohtake_i 0:ed6a60b7face 1 #include "mbed.h"
ohtake_i 0:ed6a60b7face 2 #include "QEI.h"
ohtake_i 0:ed6a60b7face 3 #include "RS405cb.h"
ohtake_i 0:ed6a60b7face 4
ohtake_i 0:ed6a60b7face 5 /*---------------------------------------------------
ohtake_i 0:ed6a60b7face 6 Created by ohtake_i
ohtake_i 0:ed6a60b7face 7 This program is for ToyBox.
ohtake_i 0:ed6a60b7face 8 Nomal routine waits for commands from PC.
ohtake_i 0:ed6a60b7face 9 ----------------------------------------------------*/
ohtake_i 0:ed6a60b7face 10
ohtake_i 0:ed6a60b7face 11 //for serials---------------------------------------------------
ohtake_i 0:ed6a60b7face 12 Serial pc(USBTX,USBRX); //for debug
ohtake_i 0:ed6a60b7face 13 Serial MyXbee(p9,p10); //for communication with pc and machine
ohtake_i 0:ed6a60b7face 14 //-----------------------------------------------------------
ohtake_i 0:ed6a60b7face 15
ohtake_i 0:ed6a60b7face 16 //for LEDs-----------------------------------------
ohtake_i 0:ed6a60b7face 17 DigitalOut debug1(LED1); //for debug
ohtake_i 0:ed6a60b7face 18 DigitalOut debug2(LED2); //for debug
ohtake_i 0:ed6a60b7face 19 DigitalOut debug3(LED3); //for debug
ohtake_i 0:ed6a60b7face 20 DigitalOut debug4(LED4); //for debug
ohtake_i 0:ed6a60b7face 21 //---------------------------------------------
ohtake_i 0:ed6a60b7face 22
ohtake_i 0:ed6a60b7face 23 //for servo-----------------------------------------
ohtake_i 1:2fddb5d4ad27 24 RS405cb servo(p13,p14,p15); //TX,RX,PERMIT (PERMIT means RE/DE Pin of ltc485)
ohtake_i 0:ed6a60b7face 25 //can control two servo motors.
ohtake_i 0:ed6a60b7face 26 //---------------------------------------------
ohtake_i 0:ed6a60b7face 27
ohtake_i 0:ed6a60b7face 28 //for Electronical Valve-----------------------------------------
ohtake_i 1:2fddb5d4ad27 29 PwmOut valve(p25);
ohtake_i 0:ed6a60b7face 30 #define VALVE_OPEN valve=1.0;
ohtake_i 0:ed6a60b7face 31 #define VALVE_CLOSE valve=0.5;
ohtake_i 0:ed6a60b7face 32 //---------------------------------------------
ohtake_i 0:ed6a60b7face 33
ohtake_i 0:ed6a60b7face 34 //for EMG30-----------------------------------------
ohtake_i 0:ed6a60b7face 35 //robot_arm
ohtake_i 0:ed6a60b7face 36 QEI arm_enc(p29, p30, NC, 624);
ohtake_i 0:ed6a60b7face 37 PwmOut arm(p22);
ohtake_i 0:ed6a60b7face 38 //jump_mechanism
ohtake_i 1:2fddb5d4ad27 39 //QEI jump_enc(p27, p28, NC, 624);
ohtake_i 0:ed6a60b7face 40 PwmOut jump(p23);
ohtake_i 0:ed6a60b7face 41 //---------------------------------------------
ohtake_i 0:ed6a60b7face 42
ohtake_i 0:ed6a60b7face 43 //for control_angle
ohtake_i 0:ed6a60b7face 44 void Control_Motor();
ohtake_i 0:ed6a60b7face 45 void Sample_Move();
ohtake_i 2:531e6b0346e6 46 //void Control_DCMotor(double desired_value, double offset, double speed_control);
ohtake_i 2:531e6b0346e6 47 void Control_DCMotor(int desired_value, int offset, double speed_control);
ohtake_i 0:ed6a60b7face 48
ohtake_i 0:ed6a60b7face 49 int main() {
ohtake_i 0:ed6a60b7face 50 while(1) {
ohtake_i 2:531e6b0346e6 51 printf("Start\r\n");
ohtake_i 2:531e6b0346e6 52 Control_DCMotor(180.0,5.0,0.05);
ohtake_i 2:531e6b0346e6 53 //Sample_Move();
ohtake_i 1:2fddb5d4ad27 54 //VALVE_OPEN;
ohtake_i 1:2fddb5d4ad27 55 //VALVE_CLOSE;
ohtake_i 1:2fddb5d4ad27 56 //servo.TORQUE_ON(1);
ohtake_i 2:531e6b0346e6 57 //servo.Rotate_Servo_Float(1,0.0);
ohtake_i 0:ed6a60b7face 58 }
ohtake_i 0:ed6a60b7face 59 }
ohtake_i 0:ed6a60b7face 60
ohtake_i 0:ed6a60b7face 61 //Servo performance test
ohtake_i 0:ed6a60b7face 62 void Sample_Move(void) {
ohtake_i 0:ed6a60b7face 63 servo.TORQUE_ON(1);
ohtake_i 0:ed6a60b7face 64 servo.TORQUE_ON(2);
ohtake_i 0:ed6a60b7face 65 while(1) {
ohtake_i 1:2fddb5d4ad27 66 servo.Rotate_Servo_Float(1,0.0);
ohtake_i 1:2fddb5d4ad27 67 servo.Rotate_Servo_Float(2,0.0);
ohtake_i 1:2fddb5d4ad27 68 wait(5);
ohtake_i 1:2fddb5d4ad27 69 for(int j=0; j<60; j++) {
ohtake_i 1:2fddb5d4ad27 70 for(int i=0; i<120; i++) {
ohtake_i 0:ed6a60b7face 71 servo.Rotate_Servo_Float(1,j*1.0);
ohtake_i 1:2fddb5d4ad27 72 servo.Rotate_Servo_Float(2,60.0-i*1.0);
ohtake_i 0:ed6a60b7face 73 wait(0.05);
ohtake_i 0:ed6a60b7face 74 }
ohtake_i 1:2fddb5d4ad27 75 for(int i=0; i<120; i++) {
ohtake_i 0:ed6a60b7face 76 servo.Rotate_Servo_Float(1,j*1.0);
ohtake_i 1:2fddb5d4ad27 77 servo.Rotate_Servo_Float(2,-60.0+i*1.0);
ohtake_i 1:2fddb5d4ad27 78 wait(0.05);
ohtake_i 0:ed6a60b7face 79 }
ohtake_i 0:ed6a60b7face 80 }
ohtake_i 0:ed6a60b7face 81 }
ohtake_i 0:ed6a60b7face 82 }
ohtake_i 0:ed6a60b7face 83
ohtake_i 0:ed6a60b7face 84 //Control z-axis angle of rotation
ohtake_i 2:531e6b0346e6 85 //void Control_DCMotor(double desired_value, double offset, double speed_control) {
ohtake_i 2:531e6b0346e6 86 void Control_DCMotor(int desired_value, int offset, double speed_control) {
ohtake_i 2:531e6b0346e6 87 //double tmp = abs(arm_enc.getPulses()); //get rev value
ohtake_i 2:531e6b0346e6 88 printf("Control_DCMotor_Start\r\n");
ohtake_i 0:ed6a60b7face 89 while(1) {
ohtake_i 2:531e6b0346e6 90 //if(desired_value - offset <= tmp && tmp <= desired_value + offset) {
ohtake_i 2:531e6b0346e6 91 if(desired_value - offset <= abs(arm_enc.getPulses()) && abs(arm_enc.getPulses()) <= desired_value + offset) {
ohtake_i 0:ed6a60b7face 92 arm = 0.5;
ohtake_i 0:ed6a60b7face 93 break;
ohtake_i 2:531e6b0346e6 94 printf("1 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
ohtake_i 0:ed6a60b7face 95 }
ohtake_i 2:531e6b0346e6 96 //else if(tmp < desired_value - offset) {
ohtake_i 2:531e6b0346e6 97 else if(abs(arm_enc.getPulses())
ohtake_i 2:531e6b0346e6 98 < desired_value - offset) {
ohtake_i 0:ed6a60b7face 99 arm = 0.5 + speed_control;
ohtake_i 2:531e6b0346e6 100 printf("2 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
ohtake_i 0:ed6a60b7face 101 } else {
ohtake_i 0:ed6a60b7face 102 arm = 0.5 - speed_control;
ohtake_i 2:531e6b0346e6 103 printf("3 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
ohtake_i 0:ed6a60b7face 104 }
ohtake_i 0:ed6a60b7face 105 }
ohtake_i 0:ed6a60b7face 106 }
ohtake_i 0:ed6a60b7face 107
ohtake_i 0:ed6a60b7face 108 //Control Command Servo & DCMotor for robot_arm
ohtake_i 0:ed6a60b7face 109 void robot_arm(double theta_1, double theta_2, double theta_3) {
ohtake_i 0:ed6a60b7face 110 //theta_1 : z-axis angle of rotation, for EMG30
ohtake_i 0:ed6a60b7face 111 //theta_2 : x-axis angle of rotation, for RS401CR
ohtake_i 0:ed6a60b7face 112 //theta_3 : x-axis angle of rotation, for RS301CR
ohtake_i 0:ed6a60b7face 113 double x, y, c[2];
ohtake_i 0:ed6a60b7face 114 while(1) {
ohtake_i 0:ed6a60b7face 115 printf("Please Enter the X-coordinate \r\n");
ohtake_i 0:ed6a60b7face 116 scanf("%lf", &c[0]);
ohtake_i 0:ed6a60b7face 117 printf("Please Enter the Y-coordinate \r\n");
ohtake_i 0:ed6a60b7face 118 scanf("%lf", &c[1]);
ohtake_i 0:ed6a60b7face 119 break;
ohtake_i 0:ed6a60b7face 120 }
ohtake_i 0:ed6a60b7face 121 x = c[0];
ohtake_i 0:ed6a60b7face 122 y = c[1];
ohtake_i 0:ed6a60b7face 123 pc.printf("x = %lf y = %lf\r\n", x, y); //for debug
ohtake_i 0:ed6a60b7face 124 }