Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@16:1be4a1c2d08a, 2022-01-22 (annotated)

Committer:
pmic
Date:
Sat Jan 22 13:25:25 2022 +0000
Revision:
16:1be4a1c2d08a
Parent:
13:41bf923cd055
Child:
17:c19b471f05cb
Updated PC_13 for user button.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:93d997d6b232 1 #include "mbed.h"
pmic 1:93d997d6b232 2 #include "platform/mbed_thread.h"
pmic 6:e1fa1a2d7483 3
pmic 6:e1fa1a2d7483 4 /* PM2_Libary */
pmic 1:93d997d6b232 5 #include "EncoderCounter.h"
pmic 1:93d997d6b232 6 #include "Servo.h"
pmic 3:d22942631cd7 7 #include "SpeedController.h"
pmic 13:41bf923cd055 8 #include "PositionController.h"
pmic 9:f10b974d01e0 9 #include "FastPWM.h"
pmic 11:af0f165f8761 10 #include "RangeFinder.h"
pmic 6:e1fa1a2d7483 11
pmic 6:e1fa1a2d7483 12 using namespace std::chrono;
pmic 6:e1fa1a2d7483 13
pmic 16:1be4a1c2d08a 14 InterruptIn user_button(PC_13);
pmic 8:9bb806a7f585 15 DigitalOut led(LED1);
pmic 6:e1fa1a2d7483 16
pmic 6:e1fa1a2d7483 17 bool executeMainTask = false;
pmic 6:e1fa1a2d7483 18 Timer user_button_timer, loop_timer;
pmic 7:c0f5bb355f41 19 int Ts_ms = 50;
pmic 6:e1fa1a2d7483 20
pmic 6:e1fa1a2d7483 21 /* declaration of custom button functions */
pmic 6:e1fa1a2d7483 22 void button_fall();
pmic 6:e1fa1a2d7483 23 void button_rise();
pmic 6:e1fa1a2d7483 24
pmic 11:af0f165f8761 25 // SHARP GP2Y0A21Y IR Sensor
pmic 11:af0f165f8761 26 // https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds
pmic 6:e1fa1a2d7483 27 /* create analog input object */
pmic 6:e1fa1a2d7483 28 AnalogIn analogIn(PC_2);
pmic 11:af0f165f8761 29 float dist_IRSensor = 0.0f;
pmic 6:e1fa1a2d7483 30
pmic 11:af0f165f8761 31 // 78:1 Metal Gearmotor 20Dx43L mm 12V CB
pmic 11:af0f165f8761 32 // https://www.pololu.com/product/3477
pmic 6:e1fa1a2d7483 33 /* create enable dc motor digital out object */
pmic 6:e1fa1a2d7483 34 DigitalOut enable_motors(PB_15);
pmic 10:c5d85e35758c 35 /* create pwm objects */
pmic 10:c5d85e35758c 36 FastPWM pwmOut_M1(PB_13);
pmic 10:c5d85e35758c 37 FastPWM pwmOut_M2(PA_9);
pmic 10:c5d85e35758c 38 FastPWM pwmOut_M3(PA_10);
pmic 10:c5d85e35758c 39 double Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
pmic 6:e1fa1a2d7483 40 /* create encoder read objects */
pmic 10:c5d85e35758c 41 EncoderCounter encoderCounter_M1(PA_6, PC_7);
pmic 10:c5d85e35758c 42 EncoderCounter encoderCounter_M2(PB_6, PB_7);
pmic 10:c5d85e35758c 43 EncoderCounter encoderCounter_M3(PA_0, PA_1);
pmic 10:c5d85e35758c 44 /* create speed controller objects, only M1 and M2, M3 is used open-loop */
pmic 10:c5d85e35758c 45 float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
pmic 10:c5d85e35758c 46 float kn = 180.0f/12.0f; // (RPM/V)
pmic 10:c5d85e35758c 47 float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used
pmic 13:41bf923cd055 48 SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
pmic 13:41bf923cd055 49 PositionController positionController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
pmic 13:41bf923cd055 50 float max_speed_rps = 1.0f; // has to be smaller or equal to kn * max_voltage
pmic 6:e1fa1a2d7483 51
pmic 11:af0f165f8761 52 // Futaba Servo S3001 20mm 3kg Analog
pmic 11:af0f165f8761 53 // https://www.modellmarkt24.ch/pi/RC-Elektronik/Servos/Standard-Servo-20mm/futaba-servo-s3001-20mm-3kg-analog.html?gclid=CjwKCAjw3pWDBhB3EiwAV1c5rK_-x_Bt19_wIY-IcS2C-RULXKBtYfY0byxejkZLjASro-EMPBUhrxoCgaQQAvD_BwE
pmic 6:e1fa1a2d7483 54 /* create servo objects */
pmic 10:c5d85e35758c 55 Servo servo_S1(PB_2);
pmic 10:c5d85e35758c 56 Servo servo_S2(PC_8);
pmic 11:af0f165f8761 57 // Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below
pmic 12:3dfd8f2939ac 58 int Ts_pwm_mus = 20000;
pmic 10:c5d85e35758c 59 int servoOutput_mus_S1 = 0;
pmic 10:c5d85e35758c 60 int servoOutput_mus_S2 = 0;
pmic 10:c5d85e35758c 61 int servo_counter = 0;
pmic 10:c5d85e35758c 62 int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
pmic 1:93d997d6b232 63
pmic 11:af0f165f8761 64 // Groove Ultrasonic Ranger V2.0
pmic 11:af0f165f8761 65 // https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds
pmic 11:af0f165f8761 66 /* create range finder object (ultra sonic distance sensor) */
pmic 11:af0f165f8761 67 RangeFinder rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
pmic 11:af0f165f8761 68 // RangeFinder rangefinder(PB_6, 5782.0f, 0.02f, 7000); // 1/Ts_ms = 50 Hz parametrization
pmic 11:af0f165f8761 69 float dist_USSensor = 0.0f;
pmic 11:af0f165f8761 70
pmic 1:93d997d6b232 71 int main()
pmic 9:f10b974d01e0 72 {
pmic 6:e1fa1a2d7483 73 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 74 user_button.rise(&button_rise);
pmic 6:e1fa1a2d7483 75 loop_timer.start();
pmic 6:e1fa1a2d7483 76
pmic 10:c5d85e35758c 77 /* enable hardwaredriver dc motors */
pmic 10:c5d85e35758c 78 enable_motors = 1;
pmic 10:c5d85e35758c 79 /* initialize pwm for motor M3*/
pmic 10:c5d85e35758c 80 pwmOut_M3.period(Ts_pwm_s);
pmic 6:e1fa1a2d7483 81 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
pmic 10:c5d85e35758c 82 pwmOut_M3.write(0.5);
pmic 9:f10b974d01e0 83
pmic 10:c5d85e35758c 84 /* enable servos, you can also disable them */
pmic 12:3dfd8f2939ac 85 servo_S1.Enable(servoOutput_mus_S1, Ts_pwm_mus);
pmic 12:3dfd8f2939ac 86 servo_S2.Enable(servoOutput_mus_S2, Ts_pwm_mus);
pmic 6:e1fa1a2d7483 87
pmic 1:93d997d6b232 88 while (true) {
pmic 6:e1fa1a2d7483 89
pmic 6:e1fa1a2d7483 90 loop_timer.reset();
pmic 6:e1fa1a2d7483 91
pmic 6:e1fa1a2d7483 92 /* ------------- start hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 93
pmic 6:e1fa1a2d7483 94 if (executeMainTask) {
pmic 6:e1fa1a2d7483 95
pmic 6:e1fa1a2d7483 96 /* read analog input */
pmic 11:af0f165f8761 97 dist_IRSensor = analogIn.read() * 3.3f;
pmic 6:e1fa1a2d7483 98
pmic 10:c5d85e35758c 99 /* command a speed to dc motors M1 and M2*/
pmic 10:c5d85e35758c 100 speedController_M1.setDesiredSpeedRPS( 1.0f);
pmic 13:41bf923cd055 101 positionController_M2.setDesiredRotation(0.5f, max_speed_rps);
pmic 10:c5d85e35758c 102 /* write output voltage to motor M3 */
pmic 10:c5d85e35758c 103 pwmOut_M3.write(0.75);
pmic 6:e1fa1a2d7483 104
pmic 10:c5d85e35758c 105 /* command servo position via output time, this needs to be calibrated */
pmic 10:c5d85e35758c 106 servo_S1.SetPosition(servoOutput_mus_S1);
pmic 10:c5d85e35758c 107 servo_S2.SetPosition(servoOutput_mus_S2);
pmic 12:3dfd8f2939ac 108 if (servoOutput_mus_S1 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 10:c5d85e35758c 109 servoOutput_mus_S1 += 100;
pmic 8:9bb806a7f585 110 }
pmic 12:3dfd8f2939ac 111 if (servoOutput_mus_S2 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 10:c5d85e35758c 112 servoOutput_mus_S2 += 100;
pmic 8:9bb806a7f585 113 }
pmic 10:c5d85e35758c 114 servo_counter++;
pmic 6:e1fa1a2d7483 115
pmic 11:af0f165f8761 116 /* read ultra sonic distance sensor */
pmic 11:af0f165f8761 117 dist_USSensor = rangeFinder.read_cm();
pmic 11:af0f165f8761 118
pmic 6:e1fa1a2d7483 119 /* visual feedback that the main task is executed */
pmic 6:e1fa1a2d7483 120 led = !led;
pmic 9:f10b974d01e0 121
pmic 1:93d997d6b232 122 } else {
pmic 6:e1fa1a2d7483 123
pmic 11:af0f165f8761 124 dist_IRSensor = 0.0f;
pmic 1:93d997d6b232 125
pmic 10:c5d85e35758c 126 speedController_M1.setDesiredSpeedRPS(0.0f);
pmic 13:41bf923cd055 127 positionController_M2.setDesiredRotation(0.0f, max_speed_rps);
pmic 10:c5d85e35758c 128 pwmOut_M3.write(0.5);
pmic 6:e1fa1a2d7483 129
pmic 10:c5d85e35758c 130 servoOutput_mus_S1 = 0;
pmic 10:c5d85e35758c 131 servoOutput_mus_S2 = 0;
pmic 10:c5d85e35758c 132 servo_S1.SetPosition(servoOutput_mus_S1);
pmic 10:c5d85e35758c 133 servo_S2.SetPosition(servoOutput_mus_S2);
pmic 11:af0f165f8761 134
pmic 11:af0f165f8761 135 dist_USSensor = 0.0f;
pmic 6:e1fa1a2d7483 136
pmic 6:e1fa1a2d7483 137 led = 0;
pmic 1:93d997d6b232 138 }
pmic 6:e1fa1a2d7483 139
pmic 10:c5d85e35758c 140 /* do only output via serial what's really necessary (this makes your code slow)*/
pmic 13:41bf923cd055 141 printf("%3.3f, %3d, %3d, %3.3f, %3.3f, %3d, %3.3f;\r\n",
pmic 11:af0f165f8761 142 dist_IRSensor,
pmic 10:c5d85e35758c 143 servoOutput_mus_S1,
pmic 10:c5d85e35758c 144 servoOutput_mus_S2,
pmic 13:41bf923cd055 145 speedController_M1.getSpeedRPS(),
pmic 13:41bf923cd055 146 positionController_M2.getRotation(),
pmic 10:c5d85e35758c 147 encoderCounter_M3.read(),
pmic 11:af0f165f8761 148 dist_USSensor);
pmic 13:41bf923cd055 149
pmic 6:e1fa1a2d7483 150 /* ------------- stop hacking ------------- -------------*/
pmic 6:e1fa1a2d7483 151
pmic 6:e1fa1a2d7483 152 int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 153 int dT_loop_ms = Ts_ms - T_loop_ms;
pmic 6:e1fa1a2d7483 154 thread_sleep_for(dT_loop_ms);
pmic 1:93d997d6b232 155 }
pmic 1:93d997d6b232 156 }
pmic 6:e1fa1a2d7483 157
pmic 6:e1fa1a2d7483 158 void button_fall()
pmic 6:e1fa1a2d7483 159 {
pmic 6:e1fa1a2d7483 160 user_button_timer.reset();
pmic 6:e1fa1a2d7483 161 user_button_timer.start();
pmic 6:e1fa1a2d7483 162 }
pmic 6:e1fa1a2d7483 163
pmic 6:e1fa1a2d7483 164 void button_rise()
pmic 6:e1fa1a2d7483 165 {
pmic 6:e1fa1a2d7483 166 int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 167 user_button_timer.stop();
pmic 8:9bb806a7f585 168 if (t_button_ms > 200) {
pmic 6:e1fa1a2d7483 169 executeMainTask = !executeMainTask;
pmic 8:9bb806a7f585 170 }
pmic 6:e1fa1a2d7483 171 }