Robocon 2021 / Mbed 2 deprecated Auto_DC_load

Auto load one arrow. Change the variable load_per_pulse to tune. after loading all the arrow, press user button again to reset (touch the firing pin).

Dependencies:   mbed QEI ros_lib_melodic

main.cpp@0:a60467f3655b, 2021-05-25 (annotated)

Committer:
ftppr
Date:
Tue May 25 08:28:25 2021 +0000
Revision:
0:a60467f3655b
Auto load one arrow;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ftppr 0:a60467f3655b 1 /***************************************************************************
ftppr 0:a60467f3655b 2 SERIAL INTERRUPT PROGRAM
ftppr 0:a60467f3655b 3 This program attaches to the serial communication link so that when a
ftppr 0:a60467f3655b 4 character is typed on the keyboard the program generates an interrupt and
ftppr 0:a60467f3655b 5 jumps to the interrupt service routine. In this program the User LED
ftppr 0:a60467f3655b 6 normally blinks ON and OFF continuously. Every time the letter "a" is
ftppr 0:a60467f3655b 7 pressed on the keyboard the LED is ON and stops blinking for 4 seconds.
ftppr 0:a60467f3655b 8 Save the file and click Compile button on the menu bar. The program will now be compiled
ftppr 0:a60467f3655b 9 In reverse, when the letter "b" is pressed the LED is OFF and the blinking
ftppr 0:a60467f3655b 10 stops for 4 seconds. After the interrupt finishes the LED blinking
ftppr 0:a60467f3655b 11 continuous.
ftppr 0:a60467f3655b 12 **************************************************************************/
ftppr 0:a60467f3655b 13 #include "mbed.h"
ftppr 0:a60467f3655b 14 #include "QEI.h"
ftppr 0:a60467f3655b 15
ftppr 0:a60467f3655b 16 //DigitalIn home_button(D13); // Button for setting the home button
ftppr 0:a60467f3655b 17 QEI dc_motor(D6, D7, NC, 792); // Create QEI object for increment encoder
ftppr 0:a60467f3655b 18
ftppr 0:a60467f3655b 19 Serial myPC(USBTX, USBRX); // import serial for the UART attached toUSB
ftppr 0:a60467f3655b 20 DigitalOut control_1(D4); // control pin on L298N - 1
ftppr 0:a60467f3655b 21 DigitalOut control_2(D5); // control pin on L298N - 2
ftppr 0:a60467f3655b 22 //PwmOut speed(D6); // pwm signal for
ftppr 0:a60467f3655b 23 //DigitalOut myLED(LED1);
ftppr 0:a60467f3655b 24 DigitalIn button(USER_BUTTON);
ftppr 0:a60467f3655b 25 DigitalIn firing(D8);
ftppr 0:a60467f3655b 26 void go_angle(int angle){ // Move motor to specific angle from home point
ftppr 0:a60467f3655b 27 int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0); // Calculating target pulse number
ftppr 0:a60467f3655b 28 int cur_pulse = dc_motor.getPulses();
ftppr 0:a60467f3655b 29
ftppr 0:a60467f3655b 30 while ( tar_pulse < cur_pulse - 100 || tar_pulse > cur_pulse + 100 ){
ftppr 0:a60467f3655b 31
ftppr 0:a60467f3655b 32 if (tar_pulse > cur_pulse){ // Rotate motor clockwise
ftppr 0:a60467f3655b 33 control_1 = 0;
ftppr 0:a60467f3655b 34 control_2 = 1;
ftppr 0:a60467f3655b 35 }
ftppr 0:a60467f3655b 36 else { // Rotate motor counter clockwrise
ftppr 0:a60467f3655b 37 control_1 = 1;
ftppr 0:a60467f3655b 38 control_2 = 0;
ftppr 0:a60467f3655b 39 }
ftppr 0:a60467f3655b 40 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
ftppr 0:a60467f3655b 41 }
ftppr 0:a60467f3655b 42 control_1 = 0; // Stop motor
ftppr 0:a60467f3655b 43 control_2 = 0;
ftppr 0:a60467f3655b 44 };
ftppr 0:a60467f3655b 45 void go_pulse(int diff){ // Move motor to specific angle from home point
ftppr 0:a60467f3655b 46 // int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0); // Calculating target pulse number
ftppr 0:a60467f3655b 47 int cur_pulse = dc_motor.getPulses();
ftppr 0:a60467f3655b 48 int tar_pulse = cur_pulse + diff;
ftppr 0:a60467f3655b 49 // control_1 = 0; // Stop motor
ftppr 0:a60467f3655b 50 // control_2 = 0;
ftppr 0:a60467f3655b 51 // wait(0.3);
ftppr 0:a60467f3655b 52 while ( abs(tar_pulse - cur_pulse) > 100){
ftppr 0:a60467f3655b 53
ftppr 0:a60467f3655b 54 if (tar_pulse > cur_pulse){ // Rotate motor clockwise
ftppr 0:a60467f3655b 55 control_1 = 0;
ftppr 0:a60467f3655b 56 control_2 = 1;
ftppr 0:a60467f3655b 57 }
ftppr 0:a60467f3655b 58 else { // Rotate motor counter clockwrise
ftppr 0:a60467f3655b 59 control_1 = 1;
ftppr 0:a60467f3655b 60 control_2 = 0;
ftppr 0:a60467f3655b 61 }
ftppr 0:a60467f3655b 62 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
ftppr 0:a60467f3655b 63 myPC.printf("tar phase is %d, cur phase is %d\r\n",tar_pulse,cur_pulse);
ftppr 0:a60467f3655b 64 }
ftppr 0:a60467f3655b 65 control_1 = 0; // Stop motor
ftppr 0:a60467f3655b 66 control_2 = 0;
ftppr 0:a60467f3655b 67 };
ftppr 0:a60467f3655b 68 void reset(DigitalIn firing_pin, QEI &dc_motor, DigitalOut out1, DigitalOut out2)
ftppr 0:a60467f3655b 69 {
ftppr 0:a60467f3655b 70 out1 = 1;
ftppr 0:a60467f3655b 71 out2 = 0;
ftppr 0:a60467f3655b 72 myPC.printf("Enter reset, \r\n");
ftppr 0:a60467f3655b 73 while(1)
ftppr 0:a60467f3655b 74 {
ftppr 0:a60467f3655b 75 if(firing_pin == 0)
ftppr 0:a60467f3655b 76 {
ftppr 0:a60467f3655b 77 myPC.printf("Firing switch detected. Stop. Record reference point %d.\r\n", dc_motor.getPulses());
ftppr 0:a60467f3655b 78 dc_motor.reset();
ftppr 0:a60467f3655b 79 out1 = 0; out2 = 0;
ftppr 0:a60467f3655b 80 while(firing_pin == 0);
ftppr 0:a60467f3655b 81 return;
ftppr 0:a60467f3655b 82 }
ftppr 0:a60467f3655b 83 }
ftppr 0:a60467f3655b 84 };
ftppr 0:a60467f3655b 85 int main()
ftppr 0:a60467f3655b 86 {
ftppr 0:a60467f3655b 87 dc_motor.reset();
ftppr 0:a60467f3655b 88 // initialize pwm
ftppr 0:a60467f3655b 89 // speed.period(0.01);
ftppr 0:a60467f3655b 90 // speed = 0.9f;
ftppr 0:a60467f3655b 91 // initialize control pins on L298N
ftppr 0:a60467f3655b 92 control_1 = 0;
ftppr 0:a60467f3655b 93 control_2 = 0;
ftppr 0:a60467f3655b 94 myPC.printf("Initialization Finished \n\r");
ftppr 0:a60467f3655b 95 myPC.printf("Firing is %d, button is %d. \n\r", firing.read(), button.read());
ftppr 0:a60467f3655b 96 // myPC.printf("control 1 is %d, control 2 is %d.\n\r", control_1, control_2);
ftppr 0:a60467f3655b 97 int cur_pulse = dc_motor.getPulses();
ftppr 0:a60467f3655b 98
ftppr 0:a60467f3655b 99 // Variables
ftppr 0:a60467f3655b 100 int ref_pulse = 0;
ftppr 0:a60467f3655b 101 int load_per_pulse = 3000; // no. of pulses for loading one arrow
ftppr 0:a60467f3655b 102 // int load_per_angle = 0;
ftppr 0:a60467f3655b 103 int big_big_pulse = 10000; // after loading 5 arrows, rotate this to reset.
ftppr 0:a60467f3655b 104 int num_arrows = 2; // number of arrows on-robot
ftppr 0:a60467f3655b 105 int cnt = 0; // for counting how many
ftppr 0:a60467f3655b 106 // Press the user button to start initialization
ftppr 0:a60467f3655b 107 while (1)
ftppr 0:a60467f3655b 108 {
ftppr 0:a60467f3655b 109 if (button == 1)
ftppr 0:a60467f3655b 110 {
ftppr 0:a60467f3655b 111 myPC.printf("Step 1: motor starts to rotate until firing pin is reached.\r\n");
ftppr 0:a60467f3655b 112 control_1 = 1;
ftppr 0:a60467f3655b 113 control_2 = 0;
ftppr 0:a60467f3655b 114 while (button == 1)
ftppr 0:a60467f3655b 115 ;
ftppr 0:a60467f3655b 116 break;
ftppr 0:a60467f3655b 117 }
ftppr 0:a60467f3655b 118
ftppr 0:a60467f3655b 119 }
ftppr 0:a60467f3655b 120 reset(firing, dc_motor, control_1,control_2);
ftppr 0:a60467f3655b 121
ftppr 0:a60467f3655b 122 while (1)
ftppr 0:a60467f3655b 123 {
ftppr 0:a60467f3655b 124 if (button == 1)
ftppr 0:a60467f3655b 125 {
ftppr 0:a60467f3655b 126 if(cnt < num_arrows)
ftppr 0:a60467f3655b 127 {
ftppr 0:a60467f3655b 128 myPC.printf("Loading No. %d arrow.\r\n", ++cnt);
ftppr 0:a60467f3655b 129 myPC.printf("Current phase is %d, target phase is %d. \r\n", dc_motor.getPulses(), dc_motor.getPulses() - load_per_pulse);
ftppr 0:a60467f3655b 130 go_pulse(-load_per_pulse);
ftppr 0:a60467f3655b 131 myPC.printf("After rotating, current phase is %d. \r\n", dc_motor.getPulses());
ftppr 0:a60467f3655b 132 while (button == 1)
ftppr 0:a60467f3655b 133 ;
ftppr 0:a60467f3655b 134 }
ftppr 0:a60467f3655b 135 else
ftppr 0:a60467f3655b 136 {
ftppr 0:a60467f3655b 137 myPC.printf("All arrows have been loaded. Reset \r\n");
ftppr 0:a60467f3655b 138 reset(firing, dc_motor, control_1,control_2);
ftppr 0:a60467f3655b 139 cnt = 0;
ftppr 0:a60467f3655b 140 }
ftppr 0:a60467f3655b 141 }
ftppr 0:a60467f3655b 142 }
ftppr 0:a60467f3655b 143 }