Robocon 2021 / Mbed 2 deprecated Auto_DC_pick_class

very stupid

Dependencies:   mbed ros_lib_melodic DC_Stepper_Controller_Lib

main_copy.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 //DigitalIn home_button(D13); // Button for setting the home button
ftppr 0:52f6905317cb 5 DC_Motor_PID dc_motor_1(D7, D6, D10, D11, 792); //M1, M2, INA, INB, PPR
ftppr 0:52f6905317cb 6 DC_Motor_PID dc_motor_2(D4, D5, D9, D8, 792); //M1, M2, INA, INB, PPR
ftppr 0:52f6905317cb 7 Serial myPC(USBTX, USBRX); // import serial for the UART attached toUSB
ftppr 0:52f6905317cb 8 // DigitalOut control_1_1(D7); // control pin on L298N - 1
ftppr 0:52f6905317cb 9 // DigitalOut control_1_2(D6); // control pin on L298N - 2
ftppr 0:52f6905317cb 10 // DigitalOut control_2_1(D4); // control pin on L298N - 1
ftppr 0:52f6905317cb 11 // DigitalOut control_2_2(D5); // control pin on L298N - 2
ftppr 0:52f6905317cb 12 // PwmOut speed(D6); // pwm signal for
ftppr 0:52f6905317cb 13 DigitalOut myLED(LED1);
ftppr 0:52f6905317cb 14 DigitalIn button(USER_BUTTON);
ftppr 0:52f6905317cb 15 // DigitalIn valve(D9, PullDown);
ftppr 0:52f6905317cb 16 DigitalIn firing1(D2);
ftppr 0:52f6905317cb 17 DigitalIn firing2(D3);
ftppr 0:52f6905317cb 18 // DigitalOut valve_1 = D12; // valve 1
ftppr 0:52f6905317cb 19 // DigitalOut valve_2 = D13; // valve 2
ftppr 0:52f6905317cb 20 void reset(DigitalIn firing_pin, DC_Motor_PID& dc_motor, int forward)
ftppr 0:52f6905317cb 21 {
ftppr 0:52f6905317cb 22 if (forward)
ftppr 0:52f6905317cb 23 {
ftppr 0:52f6905317cb 24 dc_motor.set_out(1, 0);
ftppr 0:52f6905317cb 25 }
ftppr 0:52f6905317cb 26 else
ftppr 0:52f6905317cb 27 {
ftppr 0:52f6905317cb 28 dc_motor.set_out(0, 1);
ftppr 0:52f6905317cb 29 }
ftppr 0:52f6905317cb 30
ftppr 0:52f6905317cb 31 myPC.printf("Enter reset, \r\n");
ftppr 0:52f6905317cb 32 while (1)
ftppr 0:52f6905317cb 33 {
ftppr 0:52f6905317cb 34 if (firing_pin == 0)
ftppr 0:52f6905317cb 35 {
ftppr 0:52f6905317cb 36 myPC.printf("Firing switch detected. Stop. Record reference point %d.\r\n", dc_motor.get_pulse());
ftppr 0:52f6905317cb 37 dc_motor.reset();
ftppr 0:52f6905317cb 38 dc_motor.set_out(0, 0);
ftppr 0:52f6905317cb 39 while (firing_pin == 0)
ftppr 0:52f6905317cb 40 ;
ftppr 0:52f6905317cb 41 return;
ftppr 0:52f6905317cb 42 }
ftppr 0:52f6905317cb 43 }
ftppr 0:52f6905317cb 44 };
ftppr 0:52f6905317cb 45 float pulse_to_angle(int pulses)
ftppr 0:52f6905317cb 46 {
ftppr 0:52f6905317cb 47 return pulses * 360 / 2 / 792;
ftppr 0:52f6905317cb 48 }
ftppr 0:52f6905317cb 49 float angle_to_pulse(int angle)
ftppr 0:52f6905317cb 50 {
ftppr 0:52f6905317cb 51 return angle / 360 * 2 * 792;
ftppr 0:52f6905317cb 52 }
ftppr 0:52f6905317cb 53 int main()
ftppr 0:52f6905317cb 54 {
ftppr 0:52f6905317cb 55 // DC_Motor_PID dc_motor_1(D7, D6, D10, D11, 792); //M1, M2, INA, INB, PPR
ftppr 0:52f6905317cb 56 // DC_Motor_PID dc_motor_2(D4, D5, D9, D8, 792); //M1, M2, INA, INB, PPR
ftppr 0:52f6905317cb 57 dc_motor_1.reset();
ftppr 0:52f6905317cb 58 dc_motor_2.reset();
ftppr 0:52f6905317cb 59 // initialize pwm
ftppr 0:52f6905317cb 60 // speed.period(0.01);
ftppr 0:52f6905317cb 61 // speed = 0.9f;
ftppr 0:52f6905317cb 62 // initialize control pins on L298N
ftppr 0:52f6905317cb 63 dc_motor_1.reset();
ftppr 0:52f6905317cb 64 dc_motor_2.reset();
ftppr 0:52f6905317cb 65 myPC.printf("Initialization Finished \n\r");
ftppr 0:52f6905317cb 66 myPC.printf("Firing1 is %d, Firing2 is %d, button is %d. \n\r", firing1.read(), firing2.read(), button.read()); // valve omitted
ftppr 0:52f6905317cb 67 // open catcher
ftppr 0:52f6905317cb 68 // valve_1 = 1;
ftppr 0:52f6905317cb 69 // valve_2 = 0; // tentative
ftppr 0:52f6905317cb 70 // myPC.printf("Catcher opened. valves are %d%d. \n\r", valve_1, valve_2);
ftppr 0:52f6905317cb 71
ftppr 0:52f6905317cb 72 // Variables
ftppr 0:52f6905317cb 73 int ref_pick_1 = 0;
ftppr 0:52f6905317cb 74 int ref_pick_2 = 0;
ftppr 0:52f6905317cb 75 int angle_long_1 = 0;
ftppr 0:52f6905317cb 76 int angle_short_1 = 0; // angles used in phase 1
ftppr 0:52f6905317cb 77 int angle_long_2 = 0;
ftppr 0:52f6905317cb 78 int angle_short_2 = 0; // angles used in phase 2
ftppr 0:52f6905317cb 79 int stage = 0; // stages for picking arrow
ftppr 0:52f6905317cb 80 // Press the user button to start initialization
ftppr 0:52f6905317cb 81 while (1)
ftppr 0:52f6905317cb 82 {
ftppr 0:52f6905317cb 83 if (button == 1)
ftppr 0:52f6905317cb 84 {
ftppr 0:52f6905317cb 85 while (button == 1)
ftppr 0:52f6905317cb 86 ;
ftppr 0:52f6905317cb 87 break;
ftppr 0:52f6905317cb 88 }
ftppr 0:52f6905317cb 89 }
ftppr 0:52f6905317cb 90
ftppr 0:52f6905317cb 91 myPC.printf("=========== Start configuring the long motor ==========\r\n");
ftppr 0:52f6905317cb 92 // myPC.printf("motor1 starts to rotate until firing pin is reached.\r\n");
ftppr 0:52f6905317cb 93 reset(firing1, dc_motor_1, 1);
ftppr 0:52f6905317cb 94 myPC.printf("Motor 1 reset finished. Press button to reset Motor 2.\r\n");
ftppr 0:52f6905317cb 95 // if (firing1 == 1)
ftppr 0:52f6905317cb 96 // {
ftppr 0:52f6905317cb 97 // myPC.printf("Firing1 switch detected. Stop. Record reference point_1 %d.\r\n", dc_motor_1.get_pulse());
ftppr 0:52f6905317cb 98 // control_1_1 = 0;
ftppr 0:52f6905317cb 99 // control_1_2 = 0;
ftppr 0:52f6905317cb 100 // ref_pick_1 = dc_motor_1.get_pulse(); // stop -> record or reverse????
ftppr 0:52f6905317cb 101 // while (firing1 == 1)
ftppr 0:52f6905317cb 102 // ;
ftppr 0:52f6905317cb 103 // }
ftppr 0:52f6905317cb 104 while (button == 0)
ftppr 0:52f6905317cb 105 ; // idle
ftppr 0:52f6905317cb 106 while (button == 1)
ftppr 0:52f6905317cb 107 ; // button pressed
ftppr 0:52f6905317cb 108 myPC.printf("=========== Start configuring the short motor ==========\r\n");
ftppr 0:52f6905317cb 109 // myPC.printf("motor1 starts to rotate until firing pin is reached.\r\n");
ftppr 0:52f6905317cb 110 reset(firing2, dc_motor_2, 1);
ftppr 0:52f6905317cb 111
ftppr 0:52f6905317cb 112 // if (firing2 == 1)
ftppr 0:52f6905317cb 113 // {
ftppr 0:52f6905317cb 114 // myPC.printf("firing2 switch detected. Stop. Record reference point_2 %d.\r\n", dc_motor_2.get_pulse());
ftppr 0:52f6905317cb 115 // control_2_1 = 0;
ftppr 0:52f6905317cb 116 // control_2_2 = 0;
ftppr 0:52f6905317cb 117 // ref_pick_2 = dc_motor_2.get_pulse(); // stop -> record or reverse????
ftppr 0:52f6905317cb 118 // while (firing2 == 1)
ftppr 0:52f6905317cb 119 // ;
ftppr 0:52f6905317cb 120 // }
ftppr 0:52f6905317cb 121
ftppr 0:52f6905317cb 122 myPC.printf("=========== Initialization finished. Press user button to pick up arrows ==========\r\n");
ftppr 0:52f6905317cb 123
ftppr 0:52f6905317cb 124 while (1)
ftppr 0:52f6905317cb 125 {
ftppr 0:52f6905317cb 126 if (button == 1)
ftppr 0:52f6905317cb 127 {
ftppr 0:52f6905317cb 128 myPC.printf("Start picking up arrows\r\n");
ftppr 0:52f6905317cb 129 while (button == 1)
ftppr 0:52f6905317cb 130 ;
ftppr 0:52f6905317cb 131 if (stage == 0)
ftppr 0:52f6905317cb 132 {
ftppr 0:52f6905317cb 133 myPC.printf("Begin stage 1\r\n");
ftppr 0:52f6905317cb 134 myPC.printf("Rotating long one for % degree. \r\n", angle_long_1);
ftppr 0:52f6905317cb 135 myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_1.get_pulse()), pulse_to_angle(dc_motor_1.get_pulse()) + angle_long_1);
ftppr 0:52f6905317cb 136 dc_motor_1.move_angle(angle_long_1);
ftppr 0:52f6905317cb 137 myPC.printf("Rotating short one for % degree. \r\n", angle_short_1);
ftppr 0:52f6905317cb 138 myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_2.get_pulse()), pulse_to_angle(dc_motor_2.get_pulse()) + angle_short_1);
ftppr 0:52f6905317cb 139 dc_motor_2.move_angle(angle_short_1);
ftppr 0:52f6905317cb 140 stage++;
ftppr 0:52f6905317cb 141 myPC.printf("Finished stage 1\r\n");
ftppr 0:52f6905317cb 142 }
ftppr 0:52f6905317cb 143 else if (stage == 1)
ftppr 0:52f6905317cb 144 {
ftppr 0:52f6905317cb 145 myPC.printf("Begin stage 2\r\n");
ftppr 0:52f6905317cb 146 myPC.printf("Rotating long one for % degree. \r\n", angle_long_2);
ftppr 0:52f6905317cb 147 myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_1.get_pulse()), pulse_to_angle(dc_motor_1.get_pulse()) + angle_long_2);
ftppr 0:52f6905317cb 148 dc_motor_1.move_angle(angle_long_2);
ftppr 0:52f6905317cb 149 myPC.printf("Rotating short one for % degree. \r\n", angle_short_1);
ftppr 0:52f6905317cb 150 myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_2.get_pulse()), pulse_to_angle(dc_motor_2.get_pulse()) + angle_short_2);
ftppr 0:52f6905317cb 151 dc_motor_2.move_angle(angle_short_2);
ftppr 0:52f6905317cb 152 stage++;
ftppr 0:52f6905317cb 153 myPC.printf("Finished stage 2\r\n");
ftppr 0:52f6905317cb 154 }
ftppr 0:52f6905317cb 155 }
ftppr 0:52f6905317cb 156
ftppr 0:52f6905317cb 157 // another button for controlling the valve
ftppr 0:52f6905317cb 158 // if(valve == 0)
ftppr 0:52f6905317cb 159 // {
ftppr 0:52f6905317cb 160
ftppr 0:52f6905317cb 161 // valve_1 = 1 - valve_1;
ftppr 0:52f6905317cb 162 // valve_2 = 1 - valve_2;
ftppr 0:52f6905317cb 163 // myPC.printf("Change catcher state. %d%d.\r\n", valve_1, valve_2);
ftppr 0:52f6905317cb 164 // while (valve)
ftppr 0:52f6905317cb 165 // ;
ftppr 0:52f6905317cb 166 // }
ftppr 0:52f6905317cb 167 // if finished one process, reset
ftppr 0:52f6905317cb 168 if (stage > 1)
ftppr 0:52f6905317cb 169 {
ftppr 0:52f6905317cb 170 myPC.printf("=========== Start resetting the long motor ==========\r\n");
ftppr 0:52f6905317cb 171 reset(firing1, dc_motor_1, 0); // forward = 0 -> rotate backward
ftppr 0:52f6905317cb 172 myPC.printf("Motor 1 reset finished. Press button to reset Motor 2.\r\n");
ftppr 0:52f6905317cb 173 myPC.printf("=========== Start resetting the short motor ==========\r\n");
ftppr 0:52f6905317cb 174 reset(firing2, dc_motor_2, 0);
ftppr 0:52f6905317cb 175 myPC.printf("=========== Resetting finished. Press user button to pick up arrows ==========\r\n");
ftppr 0:52f6905317cb 176 stage = 0;
ftppr 0:52f6905317cb 177 }
ftppr 0:52f6905317cb 178 }
ftppr 0:52f6905317cb 179 }*/