Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@17:c19b471f05cb, 2022-02-10 (annotated)

Committer:
pmic
Date:
Thu Feb 10 12:04:51 2022 +0000
Revision:
17:c19b471f05cb
Parent:
16:1be4a1c2d08a
Child:
20:7e7325edcf5c
Minor adjustments.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 1:93d997d6b232 1 #include "mbed.h"
pmic 17:c19b471f05cb 2 #include "PM2_Libary.h"
pmic 6:e1fa1a2d7483 3
pmic 16:1be4a1c2d08a 4 InterruptIn user_button(PC_13);
pmic 8:9bb806a7f585 5 DigitalOut led(LED1);
pmic 6:e1fa1a2d7483 6
pmic 17:c19b471f05cb 7
pmic 17:c19b471f05cb 8 bool do_execute_main = false;
pmic 6:e1fa1a2d7483 9 Timer user_button_timer, loop_timer;
pmic 17:c19b471f05cb 10 int loop_period_ms = 50;
pmic 17:c19b471f05cb 11
pmic 6:e1fa1a2d7483 12
pmic 6:e1fa1a2d7483 13 /* declaration of custom button functions */
pmic 6:e1fa1a2d7483 14 void button_fall();
pmic 6:e1fa1a2d7483 15 void button_rise();
pmic 6:e1fa1a2d7483 16
pmic 17:c19b471f05cb 17
pmic 11:af0f165f8761 18 // SHARP GP2Y0A21Y IR Sensor
pmic 6:e1fa1a2d7483 19 /* create analog input object */
pmic 17:c19b471f05cb 20 AnalogIn analog_in(PC_2);
pmic 17:c19b471f05cb 21 float dist_ir_sensor = 0.0f;
pmic 17:c19b471f05cb 22
pmic 6:e1fa1a2d7483 23
pmic 11:af0f165f8761 24 // 78:1 Metal Gearmotor 20Dx43L mm 12V CB
pmic 6:e1fa1a2d7483 25 /* create enable dc motor digital out object */
pmic 6:e1fa1a2d7483 26 DigitalOut enable_motors(PB_15);
pmic 17:c19b471f05cb 27
pmic 10:c5d85e35758c 28 /* create pwm objects */
pmic 17:c19b471f05cb 29 FastPWM pwm_M1(PB_13);
pmic 17:c19b471f05cb 30 FastPWM pwm_M2(PA_9);
pmic 17:c19b471f05cb 31 FastPWM pwm_M3(PA_10);
pmic 17:c19b471f05cb 32 float pwm_period_s = 0.00005f;
pmic 17:c19b471f05cb 33
pmic 6:e1fa1a2d7483 34 /* create encoder read objects */
pmic 17:c19b471f05cb 35 EncoderCounter encoder_M1(PA_6, PC_7);
pmic 17:c19b471f05cb 36 EncoderCounter encoder_M2(PB_6, PB_7);
pmic 17:c19b471f05cb 37 EncoderCounter encoder_M3(PA_0, PA_1);
pmic 17:c19b471f05cb 38
pmic 17:c19b471f05cb 39 /* create speed and position controller objects, default parametrization is for 78.125:1 gear */
pmic 17:c19b471f05cb 40 float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used
pmic 17:c19b471f05cb 41 float counts_per_turn = 20.0f * 78.125f; // counts/turn * gearratio
pmic 17:c19b471f05cb 42 float kn = 180.0f / 12.0f; // motor constant (RPM/V)
pmic 6:e1fa1a2d7483 43
pmic 17:c19b471f05cb 44 float k_gear = 25.0f / 78.125f; // in case you are using a dc motor with a different gear box
pmic 17:c19b471f05cb 45 float kp = 0.1f; // this is the default speed controller gain for gear box 78.125:1
pmic 17:c19b471f05cb 46
pmic 17:c19b471f05cb 47 SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear with default contoller parameters
pmic 17:c19b471f05cb 48 // SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 25:1 gear
pmic 17:c19b471f05cb 49
pmic 17:c19b471f05cb 50 PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
pmic 17:c19b471f05cb 51 // PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 25:1 gear, we need a different speed controller gain here
pmic 17:c19b471f05cb 52 float max_speed_rps = 0.5f; // has to be smaller or equal to kn * max_voltage
pmic 17:c19b471f05cb 53
pmic 17:c19b471f05cb 54
pmic 17:c19b471f05cb 55 // Futaba Servo S3001 20mm 3kg Analog
pmic 6:e1fa1a2d7483 56 /* create servo objects */
pmic 10:c5d85e35758c 57 Servo servo_S1(PB_2);
pmic 10:c5d85e35758c 58 Servo servo_S2(PC_8);
pmic 11:af0f165f8761 59 // Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below
pmic 17:c19b471f05cb 60 int servo_position_S1_mus = 0;
pmic 17:c19b471f05cb 61 int servo_position_S2_mus = 0;
pmic 17:c19b471f05cb 62 int servo_period_mus = 20000;
pmic 10:c5d85e35758c 63 int servo_counter = 0;
pmic 17:c19b471f05cb 64 int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)loop_period_ms)));
pmic 17:c19b471f05cb 65
pmic 1:93d997d6b232 66
pmic 11:af0f165f8761 67 // Groove Ultrasonic Ranger V2.0
pmic 17:c19b471f05cb 68 // 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 69 /* create range finder object (ultra sonic distance sensor) */
pmic 17:c19b471f05cb 70 // RangeFinder range_finder(PC_6, 5782.0f, 0.02f, 17500); // 1/loop_period_ms = 20 Hz parametrization
pmic 17:c19b471f05cb 71 RangeFinder range_finder(PB_6, 5782.0f, 0.02f, 7000); // 1/loop_period_ms = 50 Hz parametrization
pmic 17:c19b471f05cb 72 float dist_us_sensor = 0.0f;
pmic 17:c19b471f05cb 73
pmic 11:af0f165f8761 74
pmic 1:93d997d6b232 75 int main()
pmic 9:f10b974d01e0 76 {
pmic 17:c19b471f05cb 77 /* attach button fall and rise functions */
pmic 6:e1fa1a2d7483 78 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 79 user_button.rise(&button_rise);
pmic 17:c19b471f05cb 80
pmic 17:c19b471f05cb 81 /* start loop_timer */
pmic 6:e1fa1a2d7483 82 loop_timer.start();
pmic 6:e1fa1a2d7483 83
pmic 17:c19b471f05cb 84 /* enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled */
pmic 10:c5d85e35758c 85 enable_motors = 1;
pmic 17:c19b471f05cb 86
pmic 17:c19b471f05cb 87 /* initialize pwm for motor M1*/
pmic 17:c19b471f05cb 88 pwm_M1.period(pwm_period_s);
pmic 17:c19b471f05cb 89 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max , 0.5 -> 0 V */
pmic 17:c19b471f05cb 90 pwm_M1.write(0.5);
pmic 9:f10b974d01e0 91
pmic 10:c5d85e35758c 92 /* enable servos, you can also disable them */
pmic 17:c19b471f05cb 93 servo_S1.Enable(servo_position_S1_mus, servo_period_mus);
pmic 17:c19b471f05cb 94 servo_S2.Enable(servo_position_S2_mus, servo_period_mus);
pmic 6:e1fa1a2d7483 95
pmic 1:93d997d6b232 96 while (true) {
pmic 6:e1fa1a2d7483 97
pmic 6:e1fa1a2d7483 98 loop_timer.reset();
pmic 6:e1fa1a2d7483 99
pmic 17:c19b471f05cb 100 if (do_execute_main) {
pmic 6:e1fa1a2d7483 101
pmic 6:e1fa1a2d7483 102 /* read analog input */
pmic 17:c19b471f05cb 103 dist_ir_sensor = analog_in.read() * 3.3f;
pmic 17:c19b471f05cb 104
pmic 17:c19b471f05cb 105 /* write output voltage to motor M1 */
pmic 17:c19b471f05cb 106 pwm_M1.write(0.75);
pmic 6:e1fa1a2d7483 107
pmic 17:c19b471f05cb 108 /* set a desired speed for speed controlled dc motors M2 */
pmic 17:c19b471f05cb 109 speedController_M2.setDesiredSpeedRPS(0.5f);
pmic 17:c19b471f05cb 110
pmic 17:c19b471f05cb 111 /* set a desired rotation for position controlled dc motors M3 */
pmic 17:c19b471f05cb 112 positionController_M3.setDesiredRotation(1.5f, max_speed_rps);
pmic 17:c19b471f05cb 113
pmic 6:e1fa1a2d7483 114
pmic 10:c5d85e35758c 115 /* command servo position via output time, this needs to be calibrated */
pmic 17:c19b471f05cb 116 servo_S1.SetPosition(servo_position_S1_mus);
pmic 17:c19b471f05cb 117 servo_S2.SetPosition(servo_position_S2_mus);
pmic 17:c19b471f05cb 118 if (servo_position_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 17:c19b471f05cb 119 servo_position_S1_mus += 100;
pmic 8:9bb806a7f585 120 }
pmic 17:c19b471f05cb 121 if (servo_position_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 17:c19b471f05cb 122 servo_position_S2_mus += 100;
pmic 8:9bb806a7f585 123 }
pmic 10:c5d85e35758c 124 servo_counter++;
pmic 6:e1fa1a2d7483 125
pmic 11:af0f165f8761 126 /* read ultra sonic distance sensor */
pmic 17:c19b471f05cb 127 dist_us_sensor = range_finder.read_cm();
pmic 11:af0f165f8761 128
pmic 6:e1fa1a2d7483 129 /* visual feedback that the main task is executed */
pmic 6:e1fa1a2d7483 130 led = !led;
pmic 9:f10b974d01e0 131
pmic 1:93d997d6b232 132 } else {
pmic 6:e1fa1a2d7483 133
pmic 17:c19b471f05cb 134 dist_ir_sensor = 0.0f;
pmic 1:93d997d6b232 135
pmic 17:c19b471f05cb 136 pwm_M1.write(0.5);
pmic 17:c19b471f05cb 137 speedController_M2.setDesiredSpeedRPS(0.0f);
pmic 17:c19b471f05cb 138 positionController_M3.setDesiredRotation(0.0f, max_speed_rps);
pmic 6:e1fa1a2d7483 139
pmic 17:c19b471f05cb 140 servo_position_S1_mus = 0;
pmic 17:c19b471f05cb 141 servo_position_S2_mus = 0;
pmic 17:c19b471f05cb 142 servo_S1.SetPosition(servo_position_S1_mus);
pmic 17:c19b471f05cb 143 servo_S2.SetPosition(servo_position_S2_mus);
pmic 17:c19b471f05cb 144
pmic 17:c19b471f05cb 145 dist_us_sensor = 0.0f;
pmic 6:e1fa1a2d7483 146
pmic 6:e1fa1a2d7483 147 led = 0;
pmic 1:93d997d6b232 148 }
pmic 6:e1fa1a2d7483 149
pmic 10:c5d85e35758c 150 /* do only output via serial what's really necessary (this makes your code slow)*/
pmic 17:c19b471f05cb 151 printf("%3.3f, %3d, %3.3f, %3.3f, %3d, %3d, %3.3f;\r\n",
pmic 17:c19b471f05cb 152 dist_ir_sensor * 1e3,
pmic 17:c19b471f05cb 153 encoder_M1.read(),
pmic 17:c19b471f05cb 154 speedController_M2.getSpeedRPS(),
pmic 17:c19b471f05cb 155 positionController_M3.getRotation(),
pmic 17:c19b471f05cb 156 servo_position_S1_mus,
pmic 17:c19b471f05cb 157 servo_position_S2_mus,
pmic 17:c19b471f05cb 158 dist_us_sensor);
pmic 6:e1fa1a2d7483 159
pmic 17:c19b471f05cb 160
pmic 17:c19b471f05cb 161 /* ------------- stop hacking ----------------------------------------*/
pmic 17:c19b471f05cb 162
pmic 17:c19b471f05cb 163 int loop_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(loop_timer.elapsed_time()).count();
pmic 17:c19b471f05cb 164 thread_sleep_for(loop_period_ms - loop_time_ms);
pmic 1:93d997d6b232 165 }
pmic 1:93d997d6b232 166 }
pmic 6:e1fa1a2d7483 167
pmic 6:e1fa1a2d7483 168 void button_fall()
pmic 6:e1fa1a2d7483 169 {
pmic 6:e1fa1a2d7483 170 user_button_timer.reset();
pmic 6:e1fa1a2d7483 171 user_button_timer.start();
pmic 6:e1fa1a2d7483 172 }
pmic 6:e1fa1a2d7483 173
pmic 6:e1fa1a2d7483 174 void button_rise()
pmic 6:e1fa1a2d7483 175 {
pmic 17:c19b471f05cb 176 int user_button_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 177 user_button_timer.stop();
pmic 17:c19b471f05cb 178 if (user_button_time_ms > 200) {
pmic 17:c19b471f05cb 179 do_execute_main = !do_execute_main;
pmic 8:9bb806a7f585 180 }
pmic 6:e1fa1a2d7483 181 }