Jason Wessel / Mbed OS verBOT-repo

Repository for verBOT robot project, hopefully featuring two branches: Dev/Test and Prod.

Dependencies:   PM2_Libary Eigen

main.cpp@23:26b3a25fc637, 2022-03-14 (annotated)

Committer:
pmic
Date:
Mon Mar 14 08:31:15 2022 +0000
Revision:
23:26b3a25fc637
Parent:
22:f9370f71d17d
Child:
24:86f1a63e35a0
Small changes.

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 17:c19b471f05cb 59 int servo_position_S1_mus = 0;
pmic 17:c19b471f05cb 60 int servo_position_S2_mus = 0;
pmic 17:c19b471f05cb 61 int servo_period_mus = 20000;
pmic 10:c5d85e35758c 62 int servo_counter = 0;
pmic 17:c19b471f05cb 63 int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)loop_period_ms)));
pmic 17:c19b471f05cb 64
pmic 1:93d997d6b232 65
pmic 11:af0f165f8761 66 // Groove Ultrasonic Ranger V2.0
pmic 17:c19b471f05cb 67 // 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 68 /* create range finder object (ultra sonic distance sensor) */
pmic 23:26b3a25fc637 69 // RangeFinder range_finder(PB_12, 5782.0f, 0.02f, 17500); // 1/loop_period_ms = 20 Hz parametrization
pmic 23:26b3a25fc637 70 RangeFinder range_finder(PB_12, 5782.0f, 0.02f, 7000); // 1/loop_period_ms = 50 Hz parametrization
pmic 17:c19b471f05cb 71 float dist_us_sensor = 0.0f;
pmic 17:c19b471f05cb 72
pmic 11:af0f165f8761 73
pmic 20:7e7325edcf5c 74 // LSM9DS1 IMU
pmic 20:7e7325edcf5c 75 // create imu comunication objects, carefull: not all PES boards have an imu (chip shortage)
pmic 22:f9370f71d17d 76 // LSM9DS1 imu(PC_9, PA_8); // if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h"
pmic 20:7e7325edcf5c 77
pmic 1:93d997d6b232 78 int main()
pmic 23:26b3a25fc637 79 {
pmic 17:c19b471f05cb 80 /* attach button fall and rise functions */
pmic 6:e1fa1a2d7483 81 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 82 user_button.rise(&button_rise);
pmic 17:c19b471f05cb 83
pmic 17:c19b471f05cb 84 /* start loop_timer */
pmic 6:e1fa1a2d7483 85 loop_timer.start();
pmic 6:e1fa1a2d7483 86
pmic 17:c19b471f05cb 87 /* enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled */
pmic 10:c5d85e35758c 88 enable_motors = 1;
pmic 17:c19b471f05cb 89
pmic 17:c19b471f05cb 90 /* initialize pwm for motor M1*/
pmic 17:c19b471f05cb 91 pwm_M1.period(pwm_period_s);
pmic 17:c19b471f05cb 92 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max , 0.5 -> 0 V */
pmic 17:c19b471f05cb 93 pwm_M1.write(0.5);
pmic 9:f10b974d01e0 94
pmic 10:c5d85e35758c 95 /* enable servos, you can also disable them */
pmic 17:c19b471f05cb 96 servo_S1.Enable(servo_position_S1_mus, servo_period_mus);
pmic 17:c19b471f05cb 97 servo_S2.Enable(servo_position_S2_mus, servo_period_mus);
pmic 6:e1fa1a2d7483 98
pmic 1:93d997d6b232 99 while (true) {
pmic 6:e1fa1a2d7483 100
pmic 6:e1fa1a2d7483 101 loop_timer.reset();
pmic 6:e1fa1a2d7483 102
pmic 17:c19b471f05cb 103 if (do_execute_main) {
pmic 6:e1fa1a2d7483 104
pmic 6:e1fa1a2d7483 105 /* read analog input */
pmic 17:c19b471f05cb 106 dist_ir_sensor = analog_in.read() * 3.3f;
pmic 17:c19b471f05cb 107
pmic 17:c19b471f05cb 108 /* write output voltage to motor M1 */
pmic 17:c19b471f05cb 109 pwm_M1.write(0.75);
pmic 6:e1fa1a2d7483 110
pmic 17:c19b471f05cb 111 /* set a desired speed for speed controlled dc motors M2 */
pmic 17:c19b471f05cb 112 speedController_M2.setDesiredSpeedRPS(0.5f);
pmic 17:c19b471f05cb 113
pmic 17:c19b471f05cb 114 /* set a desired rotation for position controlled dc motors M3 */
pmic 17:c19b471f05cb 115 positionController_M3.setDesiredRotation(1.5f, max_speed_rps);
pmic 17:c19b471f05cb 116
pmic 6:e1fa1a2d7483 117
pmic 10:c5d85e35758c 118 /* command servo position via output time, this needs to be calibrated */
pmic 17:c19b471f05cb 119 servo_S1.SetPosition(servo_position_S1_mus);
pmic 17:c19b471f05cb 120 servo_S2.SetPosition(servo_position_S2_mus);
pmic 17:c19b471f05cb 121 if (servo_position_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 17:c19b471f05cb 122 servo_position_S1_mus += 100;
pmic 8:9bb806a7f585 123 }
pmic 17:c19b471f05cb 124 if (servo_position_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 17:c19b471f05cb 125 servo_position_S2_mus += 100;
pmic 8:9bb806a7f585 126 }
pmic 10:c5d85e35758c 127 servo_counter++;
pmic 6:e1fa1a2d7483 128
pmic 11:af0f165f8761 129 /* read ultra sonic distance sensor */
pmic 17:c19b471f05cb 130 dist_us_sensor = range_finder.read_cm();
pmic 11:af0f165f8761 131
pmic 6:e1fa1a2d7483 132 /* visual feedback that the main task is executed */
pmic 6:e1fa1a2d7483 133 led = !led;
pmic 9:f10b974d01e0 134
pmic 1:93d997d6b232 135 } else {
pmic 6:e1fa1a2d7483 136
pmic 17:c19b471f05cb 137 dist_ir_sensor = 0.0f;
pmic 1:93d997d6b232 138
pmic 17:c19b471f05cb 139 pwm_M1.write(0.5);
pmic 17:c19b471f05cb 140 speedController_M2.setDesiredSpeedRPS(0.0f);
pmic 17:c19b471f05cb 141 positionController_M3.setDesiredRotation(0.0f, max_speed_rps);
pmic 6:e1fa1a2d7483 142
pmic 17:c19b471f05cb 143 servo_position_S1_mus = 0;
pmic 17:c19b471f05cb 144 servo_position_S2_mus = 0;
pmic 17:c19b471f05cb 145 servo_S1.SetPosition(servo_position_S1_mus);
pmic 17:c19b471f05cb 146 servo_S2.SetPosition(servo_position_S2_mus);
pmic 17:c19b471f05cb 147
pmic 17:c19b471f05cb 148 dist_us_sensor = 0.0f;
pmic 6:e1fa1a2d7483 149
pmic 6:e1fa1a2d7483 150 led = 0;
pmic 1:93d997d6b232 151 }
pmic 6:e1fa1a2d7483 152
pmic 10:c5d85e35758c 153 /* do only output via serial what's really necessary (this makes your code slow)*/
pmic 17:c19b471f05cb 154 printf("%3.3f, %3d, %3.3f, %3.3f, %3d, %3d, %3.3f;\r\n",
pmic 17:c19b471f05cb 155 dist_ir_sensor * 1e3,
pmic 17:c19b471f05cb 156 encoder_M1.read(),
pmic 17:c19b471f05cb 157 speedController_M2.getSpeedRPS(),
pmic 17:c19b471f05cb 158 positionController_M3.getRotation(),
pmic 17:c19b471f05cb 159 servo_position_S1_mus,
pmic 17:c19b471f05cb 160 servo_position_S2_mus,
pmic 17:c19b471f05cb 161 dist_us_sensor);
pmic 6:e1fa1a2d7483 162
pmic 22:f9370f71d17d 163 /* read out the imu, the actual frames of the sensor reading needs to be figured out */
pmic 22:f9370f71d17d 164 /*
pmic 20:7e7325edcf5c 165 imu.updateGyro();
pmic 20:7e7325edcf5c 166 imu.updateAcc();
pmic 20:7e7325edcf5c 167 imu.updateMag();
pmic 20:7e7325edcf5c 168 printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n", imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),
pmic 20:7e7325edcf5c 169 imu.readAccX(), imu.readAccY(), imu.readAccZ(), imu.readMagX(), imu.readMagY(), imu.readMagZ());
pmic 22:f9370f71d17d 170 */
pmic 17:c19b471f05cb 171
pmic 17:c19b471f05cb 172 /* ------------- stop hacking ----------------------------------------*/
pmic 17:c19b471f05cb 173
pmic 17:c19b471f05cb 174 int loop_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(loop_timer.elapsed_time()).count();
pmic 17:c19b471f05cb 175 thread_sleep_for(loop_period_ms - loop_time_ms);
pmic 1:93d997d6b232 176 }
pmic 1:93d997d6b232 177 }
pmic 6:e1fa1a2d7483 178
pmic 6:e1fa1a2d7483 179 void button_fall()
pmic 6:e1fa1a2d7483 180 {
pmic 6:e1fa1a2d7483 181 user_button_timer.reset();
pmic 6:e1fa1a2d7483 182 user_button_timer.start();
pmic 6:e1fa1a2d7483 183 }
pmic 6:e1fa1a2d7483 184
pmic 6:e1fa1a2d7483 185 void button_rise()
pmic 6:e1fa1a2d7483 186 {
pmic 17:c19b471f05cb 187 int user_button_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 188 user_button_timer.stop();
pmic 17:c19b471f05cb 189 if (user_button_time_ms > 200) {
pmic 17:c19b471f05cb 190 do_execute_main = !do_execute_main;
pmic 8:9bb806a7f585 191 }
pmic 6:e1fa1a2d7483 192 }