Robocon 2021 / Mbed 2 deprecated Auto_DC_pick_class

very stupid

Dependencies:   mbed ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@0:52f6905317cb, 2021-06-17 (annotated)

Committer:
ftppr
Date:
Thu Jun 17 17:37:18 2021 +0000
Revision:
0:52f6905317cb
use stupid method

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ftppr 0:52f6905317cb 1 #include "mbed.h"
ftppr 0:52f6905317cb 2 #include "DC_Motor_Controller.h"
ftppr 0:52f6905317cb 3
ftppr 0:52f6905317cb 4 DC_Motor_PID dc_motor2(D10, D12, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works? original: D4, D7 for encoder, D12, D13 for out
ftppr 0:52f6905317cb 5
ftppr 0:52f6905317cb 6 Serial myPC(USBTX, USBRX); // import serial for the UART attached toUSB
ftppr 0:52f6905317cb 7
ftppr 0:52f6905317cb 8 DigitalOut myLED(LED1);
ftppr 0:52f6905317cb 9 DigitalIn button(USER_BUTTON);
ftppr 0:52f6905317cb 10 DigitalIn firing3 = D7; // Pick, short
ftppr 0:52f6905317cb 11 DigitalIn firing4 = D4; // Pick, long
ftppr 0:52f6905317cb 12 DigitalIn firing5 = D8;
ftppr 0:52f6905317cb 13 DigitalOut faul2_1 = A1;
ftppr 0:52f6905317cb 14 DigitalOut faul2_2 = A2;
ftppr 0:52f6905317cb 15 DigitalOut valve_1 = D0; // valve 1
ftppr 0:52f6905317cb 16 DigitalOut valve_2 = D1; // valve 2
ftppr 0:52f6905317cb 17
ftppr 0:52f6905317cb 18 int main()
ftppr 0:52f6905317cb 19 {
ftppr 0:52f6905317cb 20 firing3.mode(PullUp); // default 0
ftppr 0:52f6905317cb 21 firing4.mode(PullUp); // default 0
ftppr 0:52f6905317cb 22 firing5.mode(PullUp); // default 0
ftppr 0:52f6905317cb 23 int stage_pick = 0;
ftppr 0:52f6905317cb 24 dc_motor2.reset();
ftppr 0:52f6905317cb 25 dc_motor2.set_pid(0.008, 0, 0);
ftppr 0:52f6905317cb 26
ftppr 0:52f6905317cb 27 myPC.printf("Initialization Finished \n\r");
ftppr 0:52f6905317cb 28 myPC.printf("Firing3 is %d, Firing4 is %d, button is %d. \n\r", firing3.read(), firing4.read(), button.read()); // valve omitted
ftppr 0:52f6905317cb 29 while(1)
ftppr 0:52f6905317cb 30 {
ftppr 0:52f6905317cb 31 if(button == 1)
ftppr 0:52f6905317cb 32 {
ftppr 0:52f6905317cb 33 if(stage_pick == 0)
ftppr 0:52f6905317cb 34 {
ftppr 0:52f6905317cb 35 myPC.printf("dc_motor reset\r\n");
ftppr 0:52f6905317cb 36 dc_motor2.set_out(0.5, 0);
ftppr 0:52f6905317cb 37 while(firing3 == 0);
ftppr 0:52f6905317cb 38 if(firing3 == 1)
ftppr 0:52f6905317cb 39 {
ftppr 0:52f6905317cb 40 dc_motor2.set_out(0,0);
ftppr 0:52f6905317cb 41 myPC.printf("short: firing3 pin detected\n\r");
ftppr 0:52f6905317cb 42 }
ftppr 0:52f6905317cb 43 stage_pick++;
ftppr 0:52f6905317cb 44 valve_1 = 1; valve_2 = 0;
ftppr 0:52f6905317cb 45 }
ftppr 0:52f6905317cb 46 else if(stage_pick == 1)
ftppr 0:52f6905317cb 47 {
ftppr 0:52f6905317cb 48 myPC.printf("faulhaber 2 RefPos\n\r");
ftppr 0:52f6905317cb 49 faul2_1 = 1;
ftppr 0:52f6905317cb 50 while(firing4 == 0);
ftppr 0:52f6905317cb 51 faul2_1 = 0;
ftppr 0:52f6905317cb 52 myPC.printf("long: firing4 detected \n\r"); //內
ftppr 0:52f6905317cb 53 stage_pick++;
ftppr 0:52f6905317cb 54 }
ftppr 0:52f6905317cb 55 else if(stage_pick == 2)
ftppr 0:52f6905317cb 56 {
ftppr 0:52f6905317cb 57 myPC.printf("dc_motor rotate to pick up\n\r");
ftppr 0:52f6905317cb 58 dc_motor2.set_out(0,0.5);
ftppr 0:52f6905317cb 59 wait(0.3);
ftppr 0:52f6905317cb 60 dc_motor2.set_out(0,0);
ftppr 0:52f6905317cb 61 stage_pick++;
ftppr 0:52f6905317cb 62
ftppr 0:52f6905317cb 63 }
ftppr 0:52f6905317cb 64 else if(stage_pick == 3)
ftppr 0:52f6905317cb 65 {
ftppr 0:52f6905317cb 66 myPC.printf("faul2 to pick the arrows \n\r"); // 外
ftppr 0:52f6905317cb 67 faul2_2 = 1;
ftppr 0:52f6905317cb 68 while(firing5 == 0);
ftppr 0:52f6905317cb 69 wait(0.3);
ftppr 0:52f6905317cb 70 faul2_2 = 0;
ftppr 0:52f6905317cb 71 myPC.printf("Firing5 touched \n\r");
ftppr 0:52f6905317cb 72
ftppr 0:52f6905317cb 73 stage_pick++; // use firing 5
ftppr 0:52f6905317cb 74 }
ftppr 0:52f6905317cb 75 else if(stage_pick == 4)
ftppr 0:52f6905317cb 76 {
ftppr 0:52f6905317cb 77 myPC.printf("faul2 rotate to vertical \n\r"); // 內一啲
ftppr 0:52f6905317cb 78 faul2_1 = 1;
ftppr 0:52f6905317cb 79 wait(2.6);
ftppr 0:52f6905317cb 80 faul2_1 = 0;
ftppr 0:52f6905317cb 81 stage_pick++;
ftppr 0:52f6905317cb 82 }
ftppr 0:52f6905317cb 83 else if(stage_pick == 5)
ftppr 0:52f6905317cb 84 {
ftppr 0:52f6905317cb 85 myPC.printf("dc_motor rotate to final pos\n\r");
ftppr 0:52f6905317cb 86 dc_motor2.set_out(0,1);
ftppr 0:52f6905317cb 87 wait(0.4);
ftppr 0:52f6905317cb 88 dc_motor2.set_out(0,0);
ftppr 0:52f6905317cb 89 stage_pick++;
ftppr 0:52f6905317cb 90 }
ftppr 0:52f6905317cb 91 else if(stage_pick == 6)
ftppr 0:52f6905317cb 92 {
ftppr 0:52f6905317cb 93 myPC.printf("faul2 rotate to final pos \n\r"); // 內一啲
ftppr 0:52f6905317cb 94 faul2_1 = 1;
ftppr 0:52f6905317cb 95 wait(1.9);
ftppr 0:52f6905317cb 96 faul2_1 = 0;
ftppr 0:52f6905317cb 97 stage_pick = 0;
ftppr 0:52f6905317cb 98 }
ftppr 0:52f6905317cb 99 while(button == 1);
ftppr 0:52f6905317cb 100 wait(0.3);
ftppr 0:52f6905317cb 101 }
ftppr 0:52f6905317cb 102
ftppr 0:52f6905317cb 103 }
ftppr 0:52f6905317cb 104
ftppr 0:52f6905317cb 105
ftppr 0:52f6905317cb 106 }