MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 3:2441bcef5885
- Parent:
- 2:e570143bf35e
- Child:
- 4:d5502eb8adcd
diff -r e570143bf35e -r 2441bcef5885 main.cpp --- a/main.cpp Wed Jun 09 02:39:27 2021 +0000 +++ b/main.cpp Wed Jun 09 10:36:41 2021 +0000 @@ -43,8 +43,8 @@ DigitalOut eject_1 = A1; // valve 1 DigitalOut eject_2 = A2; // valve 2 // DC motors -DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; 轉夾 -DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; load 1 arrow +DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; load 1 arrow +DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; picker // Global Control variable char msg[200] = {0}; // msg for responser @@ -119,21 +119,24 @@ msg[strlen(msg)] = '0' + n_loaded; dc_motor1.set_out(0,1); n_loaded++; - if(firing2 == 1) - { + while(firing2 == 0); + // if(firing2 == 1) +// { if(n_loaded <= 5) { - dc_motor1.set_out(0,0.5); - wait(1.2); + dc_motor1.set_out(0,0.7); + wait(1.2); + dc_motor1.set_out(0,0); } else { strcpy(msg + len, "right arrow is pressed. "); strcpy(msg + strlen(msg), "n_loaded cleared!"); n_loaded = 0; + load_manual = 1; } - dc_motor1.set_out(0,0); - } + +// } // load_manual = 1; break; case -6: @@ -246,7 +249,7 @@ // Initiate firing pins firing1.mode(PullUp); // default 1 - firing2.mode(PullDown); //default 0 + firing2.mode(PullUp); // default 1 firing3.mode(PullUp); // default 1 // Reset dc motor @@ -254,10 +257,10 @@ dc_motor1.set_pid(0.008, 0, 0, 0.0); dc_motor2.reset(); dc_motor2.set_pid(0.008, 0, 0, 0.0); -// dc_motor2.set_out(0,0.5); +// dc_motor2.set_out(0,0.5); //picker // wait(0.3); // dc_motor2.move_angle(100); - dc_motor2.set_out(0,0); +// dc_motor2.set_out(0,1); // dc_motor1.move_angle(100); // dc_motor1.set_out(0.5,0); // -> testing only @@ -275,7 +278,7 @@ if(button == 1) { myled = 0; - if(!faul_reset) + if(!faul_reset && firing1 == 0) { down = 0; up = 1; back = 1; // 101 start = 1; @@ -283,30 +286,38 @@ } else { - dc_motor1.set_out(0,1); + dc_motor1.set_out(0,1); + load_manual = 1; } while(button == 1); wait(0.2); memset(msg, 0, sizeof(msg)); strcpy(msg, "user button is pressed, faul_reset is "); - msg[strlen(msg)] = '0' + faul_reset; + msg[strlen(msg)] = '0' + faul_reset; + strcpy(msg + strlen(msg), ", firing2 is "); + msg[strlen(msg)] = '0' + firing2.read(); + strcpy(msg + strlen(msg), ", load_manual is "); + msg[strlen(msg)] = '0' + load_manual; responser.publish(&str_msg); } if(load_manual && firing2 == 1) { - dc_motor1.set_out(0,0); + wait(0.17); + dc_motor1.set_out(0,0); + memset(msg, 0, sizeof(msg)); + strcpy(msg, "firing2 is touched: "); + msg[strlen(msg)] = '0' + firing2.read(); } - if(firing1.read() == 0 && start == 1) + if(firing1.read() == 1 && start == 1) { up = 0; down = 1; back = 1; // 110 - while(firing1.read() == 0); wait(0.3); - down = 0; back = 0; up = 0; - start = 0; memset(msg, 0, sizeof(msg)); strcpy(msg, "firing1 is touched: "); msg[strlen(msg)] = '0' + firing1.read(); responser.publish(&str_msg); + down = 0; back = 0; up = 0; + start = 0; } }