![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
gogo
main.cpp
- Committer:
- csggreen
- Date:
- 2019-04-24
- Revision:
- 1:e9e4edd823e5
- Parent:
- 0:f4444dfcd74c
File content as of revision 1:e9e4edd823e5:
#include "mbed.h" #include "AX12.h" #include <math.h> Serial device(D1, D0); DigitalOut led1(LED2); AX12 ax12(PA_9,PA_10,0x01,1000000); DigitalOut TxEn (D4); void Rx_interrupt(); void SetSerial(int c); int state_of_ST1 = 0; int state_of_ST2 = 0; int state_of_ST3 = 0; float u_j1=0; float u_j2=0; float u_j3=0; void drive_motor_1(); void drive_motor_2(); void drive_motor_3(); void calculation(); //GREEN------------------------------------------------------------- //motor set 1 DigitalOut ENA_1(PB_14); DigitalOut DIR_1(PC_4); DigitalOut PUL_1(PB_13); //motor set 2 DigitalOut ENA_2(PB_2); DigitalOut DIR_2(PB_15); DigitalOut PUL_2(PB_1); //motor set 3 DigitalOut ENA_3(PC_8); DigitalOut DIR_3(PC_5); DigitalOut PUL_3(PC_6); //Buzzer DigitalOut Buzzer(PA_12); //Vacum DigitalOut VACUM(PB_12); //Servo //Limit Switch DigitalIn LSwitch_1(PC_0); DigitalIn LSwitch_2(PC_1); DigitalIn LSwitch_3(PB_0); DigitalIn Home_Switch(PA_1); // SET VELOCITY float VelocityST_1 = 30; // rpm float VelocityST_2 = 100; // rpm float VelocityST_3 = 100; // rpm void open_vacum(){ VACUM = 1; } void close_vacum(){ VACUM = 0; } void open_buzzer(){ Buzzer = 1; } void close_buzzer(){ Buzzer = 0; } void set_home(){ if (LSwitch_2.read() != 0){ state_of_ST2=2; u_j2=180; drive_motor_2(); } if (LSwitch_1.read() != 0){ state_of_ST1 =2; u_j1=180; drive_motor_1(); state_of_ST1 =1; u_j1=45; drive_motor_1(); } if (LSwitch_3.read() != 0){ state_of_ST3=2; u_j3=180; drive_motor_3(); state_of_ST3=1; u_j3=70; drive_motor_3(); } ax12.SetGoal(90, 1); } //GREEN------------------------------------------------------------- int data_size = 16; char data[16] = {}; char package = 0; char num_data = 0; float q[4] = {}; void ConvertData2q() { char q_mode[4] = {data[3], data[6], data[9], data[12]}; char q_int[4] = {data[4], data[7], data[10], data[13]}; char q_dec[4] = {data[5], data[8], data[11], data[14]}; for(int i=0;i<4;i++) { if(q_mode[i] == 1) { q[i] = q_int[i]+q_dec[i]/100; } else if(q_mode[i] == 2) { q[i] = (-1)*(q_int[i]+q_dec[i]/100); } } } int main() { device.baud(115200); device.attach(&Rx_interrupt); close_buzzer(); close_vacum(); //set_home(); while(1) { if (package == 1) { package = 0; if(data[2]==1){ open_buzzer(); open_vacum(); set_home(); data[2]=0; } if(data[2]==2){ ConvertData2q(); state_of_ST1=data[3]; u_j1=abs(q[0]); state_of_ST2=data[6]; u_j2=abs(q[1]); state_of_ST3=data[9]; u_j3=abs(q[2]); drive_motor_1(); drive_motor_2(); drive_motor_3(); TxEn = 1; ax12.SetCRSpeed(0.1); ax12.SetGoal(abs(q[3]), 1); if(abs(q[3])==180){ open_vacum(); } if(abs(q[3])==90){ wait(3); close_vacum(); } } } } } void Rx_interrupt() { char c = device.getc(); int i = (int)c; SetSerial(i); } void SetSerial(int c) { if (num_data < 2){ if (c == 255){ data[num_data] = c; num_data++; }else{ data[num_data] = c; num_data = 0; } } else if (num_data < data_size){ data[num_data] = c; num_data++; if (num_data >= data_size){ if (data[data_size-1]==255){ num_data = 0; package = 1; } else num_data = 0; } } } void drive_motor_1(){ float round_1 = u_j1 * 8000/360;//1:10 rpm x step pluse u_j1 default 60000 float pluseforST_1 =(60/(VelocityST_1*800))/2; if (state_of_ST1==1){ ENA_1 = 1; DIR_1 = 0; for (int i=0; i< round_1; i++){ PUL_1 = 1; wait(pluseforST_1);//default 0.0005 PUL_1 = 0; wait(pluseforST_1);//default 0.0005 } //state_of_ST1 = 0; } if (state_of_ST1==2){ ENA_1 = 1; DIR_1 = 1; for (int i=0; i< round_1; i++){ PUL_1 = 1; wait(pluseforST_1);//default 0.0005 PUL_1 = 0; wait(pluseforST_1);//default 0.0005 if (LSwitch_1.read() == 0) { break; } } } } void drive_motor_2(){ float round_2 = u_j2 * 40000/360;// 1:20rpm x step pluse u_j1 default 60000 float pluseforST_2 =(60/(VelocityST_2*800))/2; if (state_of_ST2==1){ ENA_2 = 1; DIR_2 = 1; for (int i=0; i< round_2; i++){ PUL_2 = 1; wait(pluseforST_2);//default 0.0005 PUL_2 = 0; wait(pluseforST_2);//default 0.0005 } //state_of_ST1 = 0; } if (state_of_ST2==2){ ENA_2 = 1; DIR_2 = 0; for (int i=0; i< round_2; i++){ PUL_2 = 1; wait(pluseforST_2);//default 0.0005 PUL_2 = 0; wait(pluseforST_2);//default 0.0005 if (LSwitch_2.read() == 0) { break; } } } } void drive_motor_3(){ float round_3 = u_j3 * 16000 /360;//1:40 rpm x step pluse u_j1 default 60000 float pluseforST_3 =(60/(VelocityST_3*800))/2; if (state_of_ST3==1){ ENA_3 = 1; DIR_3 = 0; for (int i=0; i< round_3; i++){ PUL_3 = 1; wait(pluseforST_3);//default 0.0005 PUL_3 = 0; wait(pluseforST_3);//default 0.0005 } //state_of_ST1 = 0; } if (state_of_ST3==2){ ENA_3 = 1; DIR_3 = 1; for (int i=0; i< round_3; i++){ PUL_3 = 1; wait(pluseforST_3);//default 0.0005 PUL_3 = 0; wait(pluseforST_3);//default 0.0005 if (LSwitch_3.read() == 0) { break; } } } }