![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
gogo
main.cpp@1:e9e4edd823e5, 2019-04-24 (annotated)
- Committer:
- csggreen
- Date:
- Wed Apr 24 05:26:53 2019 +0000
- Revision:
- 1:e9e4edd823e5
- Parent:
- 0:f4444dfcd74c
g
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
csggreen | 1:e9e4edd823e5 | 1 | |
moove1334 | 0:f4444dfcd74c | 2 | #include "mbed.h" |
moove1334 | 0:f4444dfcd74c | 3 | #include "AX12.h" |
csggreen | 1:e9e4edd823e5 | 4 | #include <math.h> |
csggreen | 1:e9e4edd823e5 | 5 | Serial device(D1, D0); |
csggreen | 1:e9e4edd823e5 | 6 | DigitalOut led1(LED2); |
moove1334 | 0:f4444dfcd74c | 7 | |
moove1334 | 0:f4444dfcd74c | 8 | AX12 ax12(PA_9,PA_10,0x01,1000000); |
moove1334 | 0:f4444dfcd74c | 9 | DigitalOut TxEn (D4); |
moove1334 | 0:f4444dfcd74c | 10 | |
csggreen | 1:e9e4edd823e5 | 11 | void Rx_interrupt(); |
csggreen | 1:e9e4edd823e5 | 12 | void SetSerial(int c); |
csggreen | 1:e9e4edd823e5 | 13 | |
csggreen | 1:e9e4edd823e5 | 14 | int state_of_ST1 = 0; |
csggreen | 1:e9e4edd823e5 | 15 | int state_of_ST2 = 0; |
csggreen | 1:e9e4edd823e5 | 16 | int state_of_ST3 = 0; |
csggreen | 1:e9e4edd823e5 | 17 | |
csggreen | 1:e9e4edd823e5 | 18 | float u_j1=0; |
csggreen | 1:e9e4edd823e5 | 19 | float u_j2=0; |
csggreen | 1:e9e4edd823e5 | 20 | float u_j3=0; |
csggreen | 1:e9e4edd823e5 | 21 | |
csggreen | 1:e9e4edd823e5 | 22 | void drive_motor_1(); |
csggreen | 1:e9e4edd823e5 | 23 | void drive_motor_2(); |
csggreen | 1:e9e4edd823e5 | 24 | void drive_motor_3(); |
csggreen | 1:e9e4edd823e5 | 25 | void calculation(); |
csggreen | 1:e9e4edd823e5 | 26 | |
csggreen | 1:e9e4edd823e5 | 27 | //GREEN------------------------------------------------------------- |
csggreen | 1:e9e4edd823e5 | 28 | //motor set 1 |
csggreen | 1:e9e4edd823e5 | 29 | DigitalOut ENA_1(PB_14); |
csggreen | 1:e9e4edd823e5 | 30 | DigitalOut DIR_1(PC_4); |
csggreen | 1:e9e4edd823e5 | 31 | DigitalOut PUL_1(PB_13); |
csggreen | 1:e9e4edd823e5 | 32 | |
csggreen | 1:e9e4edd823e5 | 33 | //motor set 2 |
csggreen | 1:e9e4edd823e5 | 34 | DigitalOut ENA_2(PB_2); |
csggreen | 1:e9e4edd823e5 | 35 | DigitalOut DIR_2(PB_15); |
csggreen | 1:e9e4edd823e5 | 36 | DigitalOut PUL_2(PB_1); |
csggreen | 1:e9e4edd823e5 | 37 | |
csggreen | 1:e9e4edd823e5 | 38 | //motor set 3 |
csggreen | 1:e9e4edd823e5 | 39 | DigitalOut ENA_3(PC_8); |
csggreen | 1:e9e4edd823e5 | 40 | DigitalOut DIR_3(PC_5); |
csggreen | 1:e9e4edd823e5 | 41 | DigitalOut PUL_3(PC_6); |
csggreen | 1:e9e4edd823e5 | 42 | |
csggreen | 1:e9e4edd823e5 | 43 | //Buzzer |
csggreen | 1:e9e4edd823e5 | 44 | DigitalOut Buzzer(PA_12); |
csggreen | 1:e9e4edd823e5 | 45 | |
csggreen | 1:e9e4edd823e5 | 46 | //Vacum |
csggreen | 1:e9e4edd823e5 | 47 | DigitalOut VACUM(PB_12); |
csggreen | 1:e9e4edd823e5 | 48 | |
csggreen | 1:e9e4edd823e5 | 49 | //Servo |
csggreen | 1:e9e4edd823e5 | 50 | |
csggreen | 1:e9e4edd823e5 | 51 | //Limit Switch |
csggreen | 1:e9e4edd823e5 | 52 | DigitalIn LSwitch_1(PC_0); |
csggreen | 1:e9e4edd823e5 | 53 | DigitalIn LSwitch_2(PC_1); |
csggreen | 1:e9e4edd823e5 | 54 | DigitalIn LSwitch_3(PB_0); |
csggreen | 1:e9e4edd823e5 | 55 | DigitalIn Home_Switch(PA_1); |
csggreen | 1:e9e4edd823e5 | 56 | |
csggreen | 1:e9e4edd823e5 | 57 | // SET VELOCITY |
csggreen | 1:e9e4edd823e5 | 58 | float VelocityST_1 = 30; // rpm |
csggreen | 1:e9e4edd823e5 | 59 | float VelocityST_2 = 100; // rpm |
csggreen | 1:e9e4edd823e5 | 60 | float VelocityST_3 = 100; // rpm |
csggreen | 1:e9e4edd823e5 | 61 | |
csggreen | 1:e9e4edd823e5 | 62 | void open_vacum(){ |
csggreen | 1:e9e4edd823e5 | 63 | VACUM = 1; |
csggreen | 1:e9e4edd823e5 | 64 | } |
csggreen | 1:e9e4edd823e5 | 65 | void close_vacum(){ |
csggreen | 1:e9e4edd823e5 | 66 | VACUM = 0; |
csggreen | 1:e9e4edd823e5 | 67 | } |
csggreen | 1:e9e4edd823e5 | 68 | void open_buzzer(){ |
csggreen | 1:e9e4edd823e5 | 69 | Buzzer = 1; |
csggreen | 1:e9e4edd823e5 | 70 | } |
csggreen | 1:e9e4edd823e5 | 71 | void close_buzzer(){ |
csggreen | 1:e9e4edd823e5 | 72 | Buzzer = 0; |
csggreen | 1:e9e4edd823e5 | 73 | } |
csggreen | 1:e9e4edd823e5 | 74 | void set_home(){ |
csggreen | 1:e9e4edd823e5 | 75 | if (LSwitch_2.read() != 0){ |
csggreen | 1:e9e4edd823e5 | 76 | state_of_ST2=2; |
csggreen | 1:e9e4edd823e5 | 77 | u_j2=180; |
csggreen | 1:e9e4edd823e5 | 78 | drive_motor_2(); |
csggreen | 1:e9e4edd823e5 | 79 | } |
csggreen | 1:e9e4edd823e5 | 80 | if (LSwitch_1.read() != 0){ |
csggreen | 1:e9e4edd823e5 | 81 | state_of_ST1 =2; |
csggreen | 1:e9e4edd823e5 | 82 | u_j1=180; |
csggreen | 1:e9e4edd823e5 | 83 | drive_motor_1(); |
csggreen | 1:e9e4edd823e5 | 84 | state_of_ST1 =1; |
csggreen | 1:e9e4edd823e5 | 85 | u_j1=45; |
csggreen | 1:e9e4edd823e5 | 86 | drive_motor_1(); |
csggreen | 1:e9e4edd823e5 | 87 | } |
csggreen | 1:e9e4edd823e5 | 88 | if (LSwitch_3.read() != 0){ |
csggreen | 1:e9e4edd823e5 | 89 | state_of_ST3=2; |
csggreen | 1:e9e4edd823e5 | 90 | u_j3=180; |
csggreen | 1:e9e4edd823e5 | 91 | drive_motor_3(); |
csggreen | 1:e9e4edd823e5 | 92 | state_of_ST3=1; |
csggreen | 1:e9e4edd823e5 | 93 | u_j3=70; |
csggreen | 1:e9e4edd823e5 | 94 | drive_motor_3(); |
csggreen | 1:e9e4edd823e5 | 95 | } |
csggreen | 1:e9e4edd823e5 | 96 | ax12.SetGoal(90, 1); |
csggreen | 1:e9e4edd823e5 | 97 | } |
csggreen | 1:e9e4edd823e5 | 98 | //GREEN------------------------------------------------------------- |
csggreen | 1:e9e4edd823e5 | 99 | |
csggreen | 1:e9e4edd823e5 | 100 | int data_size = 16; |
csggreen | 1:e9e4edd823e5 | 101 | char data[16] = {}; |
csggreen | 1:e9e4edd823e5 | 102 | char package = 0; |
csggreen | 1:e9e4edd823e5 | 103 | char num_data = 0; |
csggreen | 1:e9e4edd823e5 | 104 | |
csggreen | 1:e9e4edd823e5 | 105 | float q[4] = {}; |
csggreen | 1:e9e4edd823e5 | 106 | void ConvertData2q() |
csggreen | 1:e9e4edd823e5 | 107 | { |
csggreen | 1:e9e4edd823e5 | 108 | char q_mode[4] = {data[3], data[6], data[9], data[12]}; |
csggreen | 1:e9e4edd823e5 | 109 | char q_int[4] = {data[4], data[7], data[10], data[13]}; |
csggreen | 1:e9e4edd823e5 | 110 | char q_dec[4] = {data[5], data[8], data[11], data[14]}; |
csggreen | 1:e9e4edd823e5 | 111 | for(int i=0;i<4;i++) |
csggreen | 1:e9e4edd823e5 | 112 | { |
csggreen | 1:e9e4edd823e5 | 113 | if(q_mode[i] == 1) |
csggreen | 1:e9e4edd823e5 | 114 | { |
csggreen | 1:e9e4edd823e5 | 115 | q[i] = q_int[i]+q_dec[i]/100; |
csggreen | 1:e9e4edd823e5 | 116 | } |
csggreen | 1:e9e4edd823e5 | 117 | else if(q_mode[i] == 2) |
csggreen | 1:e9e4edd823e5 | 118 | { |
csggreen | 1:e9e4edd823e5 | 119 | q[i] = (-1)*(q_int[i]+q_dec[i]/100); |
csggreen | 1:e9e4edd823e5 | 120 | } |
csggreen | 1:e9e4edd823e5 | 121 | } |
csggreen | 1:e9e4edd823e5 | 122 | } |
csggreen | 1:e9e4edd823e5 | 123 | |
csggreen | 1:e9e4edd823e5 | 124 | int main() |
csggreen | 1:e9e4edd823e5 | 125 | { |
csggreen | 1:e9e4edd823e5 | 126 | device.baud(115200); |
csggreen | 1:e9e4edd823e5 | 127 | device.attach(&Rx_interrupt); |
csggreen | 1:e9e4edd823e5 | 128 | close_buzzer(); |
csggreen | 1:e9e4edd823e5 | 129 | close_vacum(); |
csggreen | 1:e9e4edd823e5 | 130 | //set_home(); |
csggreen | 1:e9e4edd823e5 | 131 | while(1) |
csggreen | 1:e9e4edd823e5 | 132 | { |
csggreen | 1:e9e4edd823e5 | 133 | if (package == 1) |
csggreen | 1:e9e4edd823e5 | 134 | { |
csggreen | 1:e9e4edd823e5 | 135 | package = 0; |
csggreen | 1:e9e4edd823e5 | 136 | if(data[2]==1){ |
csggreen | 1:e9e4edd823e5 | 137 | open_buzzer(); |
csggreen | 1:e9e4edd823e5 | 138 | open_vacum(); |
csggreen | 1:e9e4edd823e5 | 139 | set_home(); |
csggreen | 1:e9e4edd823e5 | 140 | data[2]=0; |
csggreen | 1:e9e4edd823e5 | 141 | } |
csggreen | 1:e9e4edd823e5 | 142 | if(data[2]==2){ |
csggreen | 1:e9e4edd823e5 | 143 | ConvertData2q(); |
csggreen | 1:e9e4edd823e5 | 144 | state_of_ST1=data[3]; |
csggreen | 1:e9e4edd823e5 | 145 | u_j1=abs(q[0]); |
csggreen | 1:e9e4edd823e5 | 146 | state_of_ST2=data[6]; |
csggreen | 1:e9e4edd823e5 | 147 | u_j2=abs(q[1]); |
csggreen | 1:e9e4edd823e5 | 148 | state_of_ST3=data[9]; |
csggreen | 1:e9e4edd823e5 | 149 | u_j3=abs(q[2]); |
csggreen | 1:e9e4edd823e5 | 150 | drive_motor_1(); |
csggreen | 1:e9e4edd823e5 | 151 | drive_motor_2(); |
csggreen | 1:e9e4edd823e5 | 152 | drive_motor_3(); |
csggreen | 1:e9e4edd823e5 | 153 | TxEn = 1; |
csggreen | 1:e9e4edd823e5 | 154 | ax12.SetCRSpeed(0.1); |
csggreen | 1:e9e4edd823e5 | 155 | ax12.SetGoal(abs(q[3]), 1); |
csggreen | 1:e9e4edd823e5 | 156 | if(abs(q[3])==180){ |
csggreen | 1:e9e4edd823e5 | 157 | open_vacum(); |
csggreen | 1:e9e4edd823e5 | 158 | } |
csggreen | 1:e9e4edd823e5 | 159 | if(abs(q[3])==90){ |
csggreen | 1:e9e4edd823e5 | 160 | wait(3); |
csggreen | 1:e9e4edd823e5 | 161 | close_vacum(); |
csggreen | 1:e9e4edd823e5 | 162 | } |
csggreen | 1:e9e4edd823e5 | 163 | } |
csggreen | 1:e9e4edd823e5 | 164 | } |
csggreen | 1:e9e4edd823e5 | 165 | } |
csggreen | 1:e9e4edd823e5 | 166 | } |
csggreen | 1:e9e4edd823e5 | 167 | |
csggreen | 1:e9e4edd823e5 | 168 | void Rx_interrupt() |
csggreen | 1:e9e4edd823e5 | 169 | { |
csggreen | 1:e9e4edd823e5 | 170 | char c = device.getc(); |
csggreen | 1:e9e4edd823e5 | 171 | int i = (int)c; |
csggreen | 1:e9e4edd823e5 | 172 | SetSerial(i); |
csggreen | 1:e9e4edd823e5 | 173 | } |
csggreen | 1:e9e4edd823e5 | 174 | void SetSerial(int c) |
csggreen | 1:e9e4edd823e5 | 175 | { |
csggreen | 1:e9e4edd823e5 | 176 | if (num_data < 2){ |
csggreen | 1:e9e4edd823e5 | 177 | if (c == 255){ |
csggreen | 1:e9e4edd823e5 | 178 | data[num_data] = c; |
csggreen | 1:e9e4edd823e5 | 179 | num_data++; |
csggreen | 1:e9e4edd823e5 | 180 | }else{ |
csggreen | 1:e9e4edd823e5 | 181 | data[num_data] = c; |
csggreen | 1:e9e4edd823e5 | 182 | num_data = 0; |
csggreen | 1:e9e4edd823e5 | 183 | } |
csggreen | 1:e9e4edd823e5 | 184 | } |
csggreen | 1:e9e4edd823e5 | 185 | else if (num_data < data_size){ |
csggreen | 1:e9e4edd823e5 | 186 | data[num_data] = c; |
csggreen | 1:e9e4edd823e5 | 187 | num_data++; |
csggreen | 1:e9e4edd823e5 | 188 | if (num_data >= data_size){ |
csggreen | 1:e9e4edd823e5 | 189 | if (data[data_size-1]==255){ |
csggreen | 1:e9e4edd823e5 | 190 | num_data = 0; |
csggreen | 1:e9e4edd823e5 | 191 | package = 1; |
csggreen | 1:e9e4edd823e5 | 192 | } |
csggreen | 1:e9e4edd823e5 | 193 | else num_data = 0; |
csggreen | 1:e9e4edd823e5 | 194 | } |
csggreen | 1:e9e4edd823e5 | 195 | } |
csggreen | 1:e9e4edd823e5 | 196 | } |
csggreen | 1:e9e4edd823e5 | 197 | void drive_motor_1(){ |
csggreen | 1:e9e4edd823e5 | 198 | float round_1 = u_j1 * 8000/360;//1:10 rpm x step pluse u_j1 default 60000 |
csggreen | 1:e9e4edd823e5 | 199 | float pluseforST_1 =(60/(VelocityST_1*800))/2; |
csggreen | 1:e9e4edd823e5 | 200 | if (state_of_ST1==1){ |
csggreen | 1:e9e4edd823e5 | 201 | ENA_1 = 1; |
csggreen | 1:e9e4edd823e5 | 202 | DIR_1 = 0; |
csggreen | 1:e9e4edd823e5 | 203 | for (int i=0; i< round_1; i++){ |
csggreen | 1:e9e4edd823e5 | 204 | PUL_1 = 1; |
csggreen | 1:e9e4edd823e5 | 205 | wait(pluseforST_1);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 206 | PUL_1 = 0; |
csggreen | 1:e9e4edd823e5 | 207 | wait(pluseforST_1);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 208 | } |
csggreen | 1:e9e4edd823e5 | 209 | //state_of_ST1 = 0; |
csggreen | 1:e9e4edd823e5 | 210 | } |
csggreen | 1:e9e4edd823e5 | 211 | if (state_of_ST1==2){ |
csggreen | 1:e9e4edd823e5 | 212 | ENA_1 = 1; |
csggreen | 1:e9e4edd823e5 | 213 | DIR_1 = 1; |
csggreen | 1:e9e4edd823e5 | 214 | for (int i=0; i< round_1; i++){ |
csggreen | 1:e9e4edd823e5 | 215 | PUL_1 = 1; |
csggreen | 1:e9e4edd823e5 | 216 | wait(pluseforST_1);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 217 | PUL_1 = 0; |
csggreen | 1:e9e4edd823e5 | 218 | wait(pluseforST_1);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 219 | if (LSwitch_1.read() == 0) |
csggreen | 1:e9e4edd823e5 | 220 | { |
csggreen | 1:e9e4edd823e5 | 221 | break; |
csggreen | 1:e9e4edd823e5 | 222 | } |
csggreen | 1:e9e4edd823e5 | 223 | } |
csggreen | 1:e9e4edd823e5 | 224 | } |
csggreen | 1:e9e4edd823e5 | 225 | } |
csggreen | 1:e9e4edd823e5 | 226 | void drive_motor_2(){ |
csggreen | 1:e9e4edd823e5 | 227 | float round_2 = u_j2 * 40000/360;// 1:20rpm x step pluse u_j1 default 60000 |
csggreen | 1:e9e4edd823e5 | 228 | float pluseforST_2 =(60/(VelocityST_2*800))/2; |
csggreen | 1:e9e4edd823e5 | 229 | if (state_of_ST2==1){ |
csggreen | 1:e9e4edd823e5 | 230 | ENA_2 = 1; |
csggreen | 1:e9e4edd823e5 | 231 | DIR_2 = 1; |
csggreen | 1:e9e4edd823e5 | 232 | for (int i=0; i< round_2; i++){ |
csggreen | 1:e9e4edd823e5 | 233 | PUL_2 = 1; |
csggreen | 1:e9e4edd823e5 | 234 | wait(pluseforST_2);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 235 | PUL_2 = 0; |
csggreen | 1:e9e4edd823e5 | 236 | wait(pluseforST_2);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 237 | } |
csggreen | 1:e9e4edd823e5 | 238 | //state_of_ST1 = 0; |
csggreen | 1:e9e4edd823e5 | 239 | |
csggreen | 1:e9e4edd823e5 | 240 | } |
csggreen | 1:e9e4edd823e5 | 241 | if (state_of_ST2==2){ |
csggreen | 1:e9e4edd823e5 | 242 | ENA_2 = 1; |
csggreen | 1:e9e4edd823e5 | 243 | DIR_2 = 0; |
csggreen | 1:e9e4edd823e5 | 244 | for (int i=0; i< round_2; i++){ |
csggreen | 1:e9e4edd823e5 | 245 | PUL_2 = 1; |
csggreen | 1:e9e4edd823e5 | 246 | wait(pluseforST_2);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 247 | PUL_2 = 0; |
csggreen | 1:e9e4edd823e5 | 248 | wait(pluseforST_2);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 249 | if (LSwitch_2.read() == 0) |
csggreen | 1:e9e4edd823e5 | 250 | { |
csggreen | 1:e9e4edd823e5 | 251 | break; |
csggreen | 1:e9e4edd823e5 | 252 | } |
csggreen | 1:e9e4edd823e5 | 253 | } |
csggreen | 1:e9e4edd823e5 | 254 | } |
csggreen | 1:e9e4edd823e5 | 255 | } |
csggreen | 1:e9e4edd823e5 | 256 | |
csggreen | 1:e9e4edd823e5 | 257 | void drive_motor_3(){ |
csggreen | 1:e9e4edd823e5 | 258 | float round_3 = u_j3 * 16000 /360;//1:40 rpm x step pluse u_j1 default 60000 |
csggreen | 1:e9e4edd823e5 | 259 | float pluseforST_3 =(60/(VelocityST_3*800))/2; |
csggreen | 1:e9e4edd823e5 | 260 | if (state_of_ST3==1){ |
csggreen | 1:e9e4edd823e5 | 261 | ENA_3 = 1; |
csggreen | 1:e9e4edd823e5 | 262 | DIR_3 = 0; |
csggreen | 1:e9e4edd823e5 | 263 | for (int i=0; i< round_3; i++){ |
csggreen | 1:e9e4edd823e5 | 264 | PUL_3 = 1; |
csggreen | 1:e9e4edd823e5 | 265 | wait(pluseforST_3);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 266 | PUL_3 = 0; |
csggreen | 1:e9e4edd823e5 | 267 | wait(pluseforST_3);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 268 | } |
csggreen | 1:e9e4edd823e5 | 269 | //state_of_ST1 = 0; |
csggreen | 1:e9e4edd823e5 | 270 | } |
csggreen | 1:e9e4edd823e5 | 271 | if (state_of_ST3==2){ |
csggreen | 1:e9e4edd823e5 | 272 | ENA_3 = 1; |
csggreen | 1:e9e4edd823e5 | 273 | DIR_3 = 1; |
csggreen | 1:e9e4edd823e5 | 274 | for (int i=0; i< round_3; i++){ |
csggreen | 1:e9e4edd823e5 | 275 | PUL_3 = 1; |
csggreen | 1:e9e4edd823e5 | 276 | wait(pluseforST_3);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 277 | PUL_3 = 0; |
csggreen | 1:e9e4edd823e5 | 278 | wait(pluseforST_3);//default 0.0005 |
csggreen | 1:e9e4edd823e5 | 279 | if (LSwitch_3.read() == 0) |
csggreen | 1:e9e4edd823e5 | 280 | { |
csggreen | 1:e9e4edd823e5 | 281 | break; |
csggreen | 1:e9e4edd823e5 | 282 | } |
csggreen | 1:e9e4edd823e5 | 283 | } |
csggreen | 1:e9e4edd823e5 | 284 | } |
csggreen | 1:e9e4edd823e5 | 285 | } |