Kaustav Saha / Mbed OS PM2_Example_Summer_School

Workshop 1

Dependencies:   PM2_Libary Eigen

main.cpp@20:7e7325edcf5c, 2022-02-23 (annotated)

Committer:
pmic
Date:
Wed Feb 23 07:17:16 2022 +0000
Revision:
20:7e7325edcf5c
Parent:
17:c19b471f05cb
Child:
22:f9370f71d17d
Included driver for LSM9DS1 IMU.

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 20:7e7325edcf5c 75 // LSM9DS1 IMU
pmic 20:7e7325edcf5c 76 // create imu comunication objects, carefull: not all PES boards have an imu (chip shortage)
pmic 20:7e7325edcf5c 77 LSM9DS1 imu(PC_9, PA_8);
pmic 20:7e7325edcf5c 78
pmic 1:93d997d6b232 79 int main()
pmic 20:7e7325edcf5c 80 {
pmic 17:c19b471f05cb 81 /* attach button fall and rise functions */
pmic 6:e1fa1a2d7483 82 user_button.fall(&button_fall);
pmic 6:e1fa1a2d7483 83 user_button.rise(&button_rise);
pmic 17:c19b471f05cb 84
pmic 17:c19b471f05cb 85 /* start loop_timer */
pmic 6:e1fa1a2d7483 86 loop_timer.start();
pmic 6:e1fa1a2d7483 87
pmic 17:c19b471f05cb 88 /* enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled */
pmic 10:c5d85e35758c 89 enable_motors = 1;
pmic 17:c19b471f05cb 90
pmic 17:c19b471f05cb 91 /* initialize pwm for motor M1*/
pmic 17:c19b471f05cb 92 pwm_M1.period(pwm_period_s);
pmic 17:c19b471f05cb 93 /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max , 0.5 -> 0 V */
pmic 17:c19b471f05cb 94 pwm_M1.write(0.5);
pmic 9:f10b974d01e0 95
pmic 10:c5d85e35758c 96 /* enable servos, you can also disable them */
pmic 17:c19b471f05cb 97 servo_S1.Enable(servo_position_S1_mus, servo_period_mus);
pmic 17:c19b471f05cb 98 servo_S2.Enable(servo_position_S2_mus, servo_period_mus);
pmic 6:e1fa1a2d7483 99
pmic 1:93d997d6b232 100 while (true) {
pmic 6:e1fa1a2d7483 101
pmic 6:e1fa1a2d7483 102 loop_timer.reset();
pmic 6:e1fa1a2d7483 103
pmic 17:c19b471f05cb 104 if (do_execute_main) {
pmic 6:e1fa1a2d7483 105
pmic 6:e1fa1a2d7483 106 /* read analog input */
pmic 17:c19b471f05cb 107 dist_ir_sensor = analog_in.read() * 3.3f;
pmic 17:c19b471f05cb 108
pmic 17:c19b471f05cb 109 /* write output voltage to motor M1 */
pmic 17:c19b471f05cb 110 pwm_M1.write(0.75);
pmic 6:e1fa1a2d7483 111
pmic 17:c19b471f05cb 112 /* set a desired speed for speed controlled dc motors M2 */
pmic 17:c19b471f05cb 113 speedController_M2.setDesiredSpeedRPS(0.5f);
pmic 17:c19b471f05cb 114
pmic 17:c19b471f05cb 115 /* set a desired rotation for position controlled dc motors M3 */
pmic 17:c19b471f05cb 116 positionController_M3.setDesiredRotation(1.5f, max_speed_rps);
pmic 17:c19b471f05cb 117
pmic 6:e1fa1a2d7483 118
pmic 10:c5d85e35758c 119 /* command servo position via output time, this needs to be calibrated */
pmic 17:c19b471f05cb 120 servo_S1.SetPosition(servo_position_S1_mus);
pmic 17:c19b471f05cb 121 servo_S2.SetPosition(servo_position_S2_mus);
pmic 17:c19b471f05cb 122 if (servo_position_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 17:c19b471f05cb 123 servo_position_S1_mus += 100;
pmic 8:9bb806a7f585 124 }
pmic 17:c19b471f05cb 125 if (servo_position_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
pmic 17:c19b471f05cb 126 servo_position_S2_mus += 100;
pmic 8:9bb806a7f585 127 }
pmic 10:c5d85e35758c 128 servo_counter++;
pmic 6:e1fa1a2d7483 129
pmic 11:af0f165f8761 130 /* read ultra sonic distance sensor */
pmic 17:c19b471f05cb 131 dist_us_sensor = range_finder.read_cm();
pmic 11:af0f165f8761 132
pmic 6:e1fa1a2d7483 133 /* visual feedback that the main task is executed */
pmic 6:e1fa1a2d7483 134 led = !led;
pmic 9:f10b974d01e0 135
pmic 1:93d997d6b232 136 } else {
pmic 6:e1fa1a2d7483 137
pmic 17:c19b471f05cb 138 dist_ir_sensor = 0.0f;
pmic 1:93d997d6b232 139
pmic 17:c19b471f05cb 140 pwm_M1.write(0.5);
pmic 17:c19b471f05cb 141 speedController_M2.setDesiredSpeedRPS(0.0f);
pmic 17:c19b471f05cb 142 positionController_M3.setDesiredRotation(0.0f, max_speed_rps);
pmic 6:e1fa1a2d7483 143
pmic 17:c19b471f05cb 144 servo_position_S1_mus = 0;
pmic 17:c19b471f05cb 145 servo_position_S2_mus = 0;
pmic 17:c19b471f05cb 146 servo_S1.SetPosition(servo_position_S1_mus);
pmic 17:c19b471f05cb 147 servo_S2.SetPosition(servo_position_S2_mus);
pmic 17:c19b471f05cb 148
pmic 17:c19b471f05cb 149 dist_us_sensor = 0.0f;
pmic 6:e1fa1a2d7483 150
pmic 6:e1fa1a2d7483 151 led = 0;
pmic 1:93d997d6b232 152 }
pmic 6:e1fa1a2d7483 153
pmic 10:c5d85e35758c 154 /* do only output via serial what's really necessary (this makes your code slow)*/
pmic 20:7e7325edcf5c 155 /*
pmic 17:c19b471f05cb 156 printf("%3.3f, %3d, %3.3f, %3.3f, %3d, %3d, %3.3f;\r\n",
pmic 17:c19b471f05cb 157 dist_ir_sensor * 1e3,
pmic 17:c19b471f05cb 158 encoder_M1.read(),
pmic 17:c19b471f05cb 159 speedController_M2.getSpeedRPS(),
pmic 17:c19b471f05cb 160 positionController_M3.getRotation(),
pmic 17:c19b471f05cb 161 servo_position_S1_mus,
pmic 17:c19b471f05cb 162 servo_position_S2_mus,
pmic 17:c19b471f05cb 163 dist_us_sensor);
pmic 20:7e7325edcf5c 164 */
pmic 6:e1fa1a2d7483 165
pmic 20:7e7325edcf5c 166 imu.updateGyro();
pmic 20:7e7325edcf5c 167 imu.updateAcc();
pmic 20:7e7325edcf5c 168 imu.updateMag();
pmic 20:7e7325edcf5c 169 printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n", imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),
pmic 20:7e7325edcf5c 170 imu.readAccX(), imu.readAccY(), imu.readAccZ(), imu.readMagX(), imu.readMagY(), imu.readMagZ());
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 }