main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "RS405cb.h"
00004 
00005 /*---------------------------------------------------
00006 Created by ohtake_i
00007 This program is for ToyBox.
00008 Nomal routine waits for commands from PC.
00009 ----------------------------------------------------*/
00010 
00011 //for serials---------------------------------------------------
00012     Serial pc(USBTX,USBRX); //for debug
00013     Serial MyXbee(p9,p10); //for communication with pc and machine
00014 //-----------------------------------------------------------
00015 
00016 //for LEDs-----------------------------------------
00017     DigitalOut debug1(LED1); //for debug
00018     DigitalOut debug2(LED2); //for debug
00019     DigitalOut debug3(LED3); //for debug
00020     DigitalOut debug4(LED4); //for debug
00021 //---------------------------------------------
00022 
00023 //for servo-----------------------------------------
00024     RS405cb servo(p13,p14,p15);    //TX,RX,PERMIT  (PERMIT means RE/DE Pin of ltc485)
00025                                //can control two servo motors.
00026 //---------------------------------------------
00027 
00028 //for Electronical Valve-----------------------------------------
00029     PwmOut valve(p25);
00030     #define VALVE_OPEN valve=1.0;
00031     #define VALVE_CLOSE valve=0.5;
00032 //---------------------------------------------
00033 
00034 //for EMG30-----------------------------------------
00035     //robot_arm
00036     QEI arm_enc(p29, p30, NC, 624);
00037     PwmOut arm(p22);
00038     //jump_mechanism
00039     //QEI jump_enc(p27, p28, NC, 624);
00040     PwmOut jump(p23);
00041 //---------------------------------------------
00042 
00043 //for control_angle
00044     void Control_Motor();
00045     void Sample_Move();
00046     //void Control_DCMotor(double desired_value, double offset, double speed_control);
00047     void Control_DCMotor(int desired_value, int offset, double speed_control);
00048 
00049 int main() {
00050     while(1) {
00051         printf("Start\r\n");
00052         Control_DCMotor(180.0,5.0,0.05);
00053         //Sample_Move();
00054         //VALVE_OPEN;
00055         //VALVE_CLOSE;
00056         //servo.TORQUE_ON(1);
00057         //servo.Rotate_Servo_Float(1,0.0);   
00058     }
00059 }
00060 
00061 //Servo performance test
00062 void Sample_Move(void) {
00063     servo.TORQUE_ON(1);
00064     servo.TORQUE_ON(2);
00065     while(1) {
00066         servo.Rotate_Servo_Float(1,0.0);
00067         servo.Rotate_Servo_Float(2,0.0);
00068         wait(5);
00069         for(int j=0; j<60; j++) {
00070             for(int i=0; i<120; i++) {
00071                 servo.Rotate_Servo_Float(1,j*1.0);
00072                 servo.Rotate_Servo_Float(2,60.0-i*1.0);
00073                 wait(0.05);
00074             }
00075             for(int i=0; i<120; i++) {
00076                 servo.Rotate_Servo_Float(1,j*1.0);
00077                 servo.Rotate_Servo_Float(2,-60.0+i*1.0);
00078                 wait(0.05);
00079             }
00080         }
00081     }
00082 }
00083 
00084 //Control z-axis angle of rotation
00085 //void Control_DCMotor(double desired_value, double offset, double speed_control) {
00086 void Control_DCMotor(int desired_value, int offset, double speed_control) {
00087     //double tmp = abs(arm_enc.getPulses());  //get rev value
00088     printf("Control_DCMotor_Start\r\n");
00089     while(1) {
00090         //if(desired_value - offset <= tmp && tmp <= desired_value + offset) {
00091         if(desired_value - offset <= abs(arm_enc.getPulses()) && abs(arm_enc.getPulses()) <= desired_value + offset) {
00092             arm = 0.5;
00093             break;
00094             printf("1 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
00095         } 
00096         //else if(tmp < desired_value - offset) {
00097         else if(abs(arm_enc.getPulses())
00098          < desired_value - offset) {
00099             arm = 0.5 + speed_control;
00100             printf("2 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
00101         } else {
00102             arm = 0.5 - speed_control;
00103             printf("3 Rotation_of_Angle : %d\r\n", abs(arm_enc.getPulses()));
00104         }
00105     }
00106 } 
00107  
00108 //Control Command Servo & DCMotor for robot_arm
00109 void robot_arm(double theta_1, double theta_2, double theta_3) {
00110     //theta_1 : z-axis angle of rotation, for EMG30
00111     //theta_2 : x-axis angle of rotation, for RS401CR
00112     //theta_3 : x-axis angle of rotation, for RS301CR
00113     double x, y, c[2];
00114     while(1) {
00115         printf("Please Enter the X-coordinate \r\n");
00116         scanf("%lf", &c[0]);
00117         printf("Please Enter the Y-coordinate \r\n");
00118         scanf("%lf", &c[1]);
00119         break;
00120     }
00121     x = c[0];
00122     y = c[1];
00123     pc.printf("x = %lf  y = %lf\r\n", x, y);    //for debug
00124 }