main.cpp

00001 #include "mbed.h"
00002 #include "AX12.h"
00003 #include <math.h>
00004 Serial device(D1, D0);
00005 DigitalOut led1(LED2);
00006 
00007 Timer t1,t2,t3;
00008 
00009 void Rx_interrupt();
00010 void SetSerial(int c);
00011 
00012 int state_of_ST1 = 0;
00013 int state_of_ST2 = 0;
00014 int state_of_ST3 = 0;
00015 
00016 float u_j1=0;
00017 float u_j2=0;
00018 float u_j3=0;
00019 
00020 void drive_motor_1();
00021 void drive_motor_2();
00022 void drive_motor_3();
00023 void calculation();
00024 
00025 //GREEN-------------------------------------------------------------
00026 //motor set 1
00027 DigitalOut ENA_1(PB_14);
00028 DigitalOut DIR_1(PB_13);
00029 DigitalOut PUL_1(PC_4);
00030 
00031 //motor set 2
00032 DigitalOut ENA_2(PB_2);
00033 DigitalOut DIR_2(PB_1);
00034 DigitalOut PUL_2(PB_15);
00035 
00036 //motor set 3
00037 DigitalOut ENA_3(PC_8);
00038 DigitalOut DIR_3(PC_6);
00039 DigitalOut PUL_3(PC_5);
00040 
00041 //Buzzer
00042 DigitalOut Buzzer(PA_12);
00043 
00044 //Vacum
00045 DigitalOut VACUM(PB_12);
00046 
00047 //Servo
00048 DigitalOut DYNA(PA_11);
00049 
00050 //Limit Switch
00051 DigitalIn LSwitch_1(PC_0);
00052 DigitalIn LSwitch_2(PC_1);
00053 DigitalIn LSwitch_3(PB_0);
00054 DigitalIn Home_Switch(PA_1);
00055 
00056 // SET VELOCITY
00057 float VelocityST_1 = 30; // rpm
00058 float VelocityST_2 = 100; // rpm
00059 float VelocityST_3 = 100; // rpm
00060 
00061 void open_vacum(){
00062     VACUM = 1;
00063 }
00064 void close_vacum(){
00065     VACUM = 0;
00066 }
00067 void open_buzzer(){
00068     Buzzer = 1;
00069 }
00070 void close_buzzer(){
00071     Buzzer = 0;
00072 }
00073 void open_servo(){
00074     DYNA = 1;
00075 }
00076 void close_servo(){
00077     DYNA = 0;
00078 }
00079 void set_home(){
00080     if (LSwitch_2.read() != 0){
00081         state_of_ST2=2;
00082         u_j1=180;          
00083         drive_motor_2();
00084     }
00085     if (LSwitch_1.read() != 0){    
00086         u_j1=180;
00087         state_of_ST1 =2;
00088         drive_motor_1(); 
00089     }
00090     if (LSwitch_3.read() != 0){
00091         state_of_ST3=2;
00092         u_j3=180;
00093         drive_motor_3();       
00094     }
00095     }
00096 //GREEN-------------------------------------------------------------
00097 
00098 int data_size = 16;
00099 char data[16] = {};
00100 char package = 0;
00101 char num_data = 0;
00102 
00103 float q[4] = {};
00104 void ConvertData2q()
00105 {
00106     char q_mode[4] = {data[3], data[6], data[9], data[12]};
00107     char q_int[4] = {data[4], data[7], data[10], data[13]};
00108     char q_dec[4] = {data[5], data[8], data[11], data[14]};
00109     for(int i=0;i<4;i++)
00110     {
00111         if(q_mode[i] == 1)
00112         {
00113             q[i] = q_int[i]+q_dec[i]/100;
00114         }
00115         else if(q_mode[i] == 2)
00116         {
00117             q[i] = (-1)*(q_int[i]+q_dec[i]/100);
00118         }
00119     }
00120 }
00121 
00122 int main()
00123 {
00124     device.baud(115200);
00125     device.attach(&Rx_interrupt);
00126     close_buzzer();
00127     close_vacum();
00128     while(1)
00129     {
00130         if (package == 1)
00131         {
00132             ConvertData2q();
00133             package = 0;
00134             state_of_ST1=data[3];
00135             u_j1=abs(q[0]);
00136             state_of_ST2=data[6];
00137             u_j2=abs(q[1]);
00138             state_of_ST3=data[9];
00139             u_j3=abs(q[2]);
00140             calculation();
00141             drive_motor_1();
00142             drive_motor_2();
00143             drive_motor_3();
00144         }
00145         if(Home_Switch.read()==0){
00146             //set_home();
00147         }
00148     }
00149 }
00150 
00151 void Rx_interrupt()
00152 {
00153     char c = device.getc();
00154     int i = (int)c;
00155     SetSerial(i);
00156 }
00157 void SetSerial(int c)
00158 {
00159     if (num_data < 2){
00160         if (c == 255){
00161             data[num_data] = c;
00162             num_data++;
00163         }else{
00164             data[num_data] = c;
00165             num_data = 0;
00166         }
00167     }
00168     else if (num_data < data_size){
00169         data[num_data] = c;
00170         num_data++;
00171         if (num_data >= data_size){
00172             if (data[data_size-1]==255){
00173                 num_data = 0;
00174                 package = 1;
00175             }
00176             else num_data = 0;
00177         }
00178     }
00179 }
00180 void drive_motor_1(){
00181         float round_1 = u_j1 * 8000/360;//1:10 rpm x step pluse u_j1 default 60000
00182         float pluseforST_1 =(60/(VelocityST_1*800))/2;
00183         ENA_1 = 1;
00184             for (int i=0; i< round_1; i++){
00185                 PUL_1 = 1;
00186                 wait(pluseforST_1);//default 0.0005
00187                 PUL_1 = 0;
00188                 wait(pluseforST_1);//default 0.0005
00189                 if (LSwitch_1.read() == 0&&state_of_ST1==2)
00190                 {
00191                     break;
00192                 }
00193             }
00194         }
00195 void drive_motor_2(){
00196             float round_2 = u_j2 * 40000/360;// 1:20rpm x step pluse u_j1 default 60000
00197             float pluseforST_2 =(60/(VelocityST_2*800))/2;
00198             ENA_2 = 1;
00199                 for (int i=0; i< round_2; i++){
00200                     PUL_2 = 1;
00201                     wait(pluseforST_2);//default 0.0005
00202                     PUL_2 = 0;
00203                     wait(pluseforST_2);//default 0.0005
00204                     if (LSwitch_2.read() == 0&&state_of_ST2==2)
00205                 {
00206                         break;
00207                 }
00208             }
00209         }
00210 
00211 void drive_motor_3(){
00212         float round_3 = u_j3 * 16000 /360;//1:40 rpm x step pluse u_j1 default 60000
00213         float pluseforST_3 =(60/(VelocityST_3*800))/2;
00214         ENA_3 = 1;
00215             for (int i=0; i< round_3; i++){
00216                 PUL_3 = 1;
00217                 wait(pluseforST_3);//default 0.0005
00218                 PUL_3 = 0;
00219                 wait(pluseforST_3);//default 0.0005
00220                 if (LSwitch_3.read() == 0&&state_of_ST3==2)
00221                 {
00222                     break;
00223                 }
00224             }
00225         }
00226 void calculation(){
00227     if (state_of_ST1==1){
00228             DIR_1 = 0;
00229     }
00230     if (state_of_ST1==2){
00231             DIR_1 = 1;  
00232     }
00233     if (state_of_ST2==1){
00234             DIR_2 = 1;
00235     }
00236     if (state_of_ST2==2){
00237             DIR_2 = 0;
00238     }
00239     if (state_of_ST3==1){
00240             DIR_3 = 0;
00241     }
00242     if (state_of_ST3==2){
00243             DIR_3 = 1;
00244     }
00245 }