main.cpp

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