Workshop 1
Dependencies: PM2_Libary Eigen
main.cpp
- Committer:
- pmic
- Date:
- 2022-03-02
- Revision:
- 22:f9370f71d17d
- Parent:
- 20:7e7325edcf5c
- Child:
- 23:26b3a25fc637
File content as of revision 22:f9370f71d17d:
#include "mbed.h" #include "PM2_Libary.h" InterruptIn user_button(PC_13); DigitalOut led(LED1); bool do_execute_main = false; Timer user_button_timer, loop_timer; int loop_period_ms = 50; /* declaration of custom button functions */ void button_fall(); void button_rise(); // SHARP GP2Y0A21Y IR Sensor /* create analog input object */ AnalogIn analog_in(PC_2); float dist_ir_sensor = 0.0f; // 78:1 Metal Gearmotor 20Dx43L mm 12V CB /* create enable dc motor digital out object */ DigitalOut enable_motors(PB_15); /* create pwm objects */ FastPWM pwm_M1(PB_13); FastPWM pwm_M2(PA_9); FastPWM pwm_M3(PA_10); float pwm_period_s = 0.00005f; /* create encoder read objects */ EncoderCounter encoder_M1(PA_6, PC_7); EncoderCounter encoder_M2(PB_6, PB_7); EncoderCounter encoder_M3(PA_0, PA_1); /* create speed and position controller objects, default parametrization is for 78.125:1 gear */ float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used float counts_per_turn = 20.0f * 78.125f; // counts/turn * gearratio float kn = 180.0f / 12.0f; // motor constant (RPM/V) float k_gear = 25.0f / 78.125f; // in case you are using a dc motor with a different gear box float kp = 0.1f; // this is the default speed controller gain for gear box 78.125:1 SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear with default contoller parameters // SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 25:1 gear PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters // 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 float max_speed_rps = 0.5f; // has to be smaller or equal to kn * max_voltage // Futaba Servo S3001 20mm 3kg Analog /* create servo objects */ Servo servo_S1(PB_2); Servo servo_S2(PC_8); // Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below int servo_position_S1_mus = 0; int servo_position_S2_mus = 0; int servo_period_mus = 20000; int servo_counter = 0; int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)loop_period_ms))); // Groove Ultrasonic Ranger V2.0 // 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 /* create range finder object (ultra sonic distance sensor) */ // RangeFinder range_finder(PC_6, 5782.0f, 0.02f, 17500); // 1/loop_period_ms = 20 Hz parametrization RangeFinder range_finder(PB_6, 5782.0f, 0.02f, 7000); // 1/loop_period_ms = 50 Hz parametrization float dist_us_sensor = 0.0f; // LSM9DS1 IMU // create imu comunication objects, carefull: not all PES boards have an imu (chip shortage) // LSM9DS1 imu(PC_9, PA_8); // if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h" int main() { /* attach button fall and rise functions */ user_button.fall(&button_fall); user_button.rise(&button_rise); /* start loop_timer */ loop_timer.start(); /* enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled */ enable_motors = 1; /* initialize pwm for motor M1*/ pwm_M1.period(pwm_period_s); /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max , 0.5 -> 0 V */ pwm_M1.write(0.5); /* enable servos, you can also disable them */ servo_S1.Enable(servo_position_S1_mus, servo_period_mus); servo_S2.Enable(servo_position_S2_mus, servo_period_mus); while (true) { loop_timer.reset(); if (do_execute_main) { /* read analog input */ dist_ir_sensor = analog_in.read() * 3.3f; /* write output voltage to motor M1 */ pwm_M1.write(0.75); /* set a desired speed for speed controlled dc motors M2 */ speedController_M2.setDesiredSpeedRPS(0.5f); /* set a desired rotation for position controlled dc motors M3 */ positionController_M3.setDesiredRotation(1.5f, max_speed_rps); /* command servo position via output time, this needs to be calibrated */ servo_S1.SetPosition(servo_position_S1_mus); servo_S2.SetPosition(servo_position_S2_mus); if (servo_position_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) { servo_position_S1_mus += 100; } if (servo_position_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) { servo_position_S2_mus += 100; } servo_counter++; /* read ultra sonic distance sensor */ dist_us_sensor = range_finder.read_cm(); /* visual feedback that the main task is executed */ led = !led; } else { dist_ir_sensor = 0.0f; pwm_M1.write(0.5); speedController_M2.setDesiredSpeedRPS(0.0f); positionController_M3.setDesiredRotation(0.0f, max_speed_rps); servo_position_S1_mus = 0; servo_position_S2_mus = 0; servo_S1.SetPosition(servo_position_S1_mus); servo_S2.SetPosition(servo_position_S2_mus); dist_us_sensor = 0.0f; led = 0; } /* do only output via serial what's really necessary (this makes your code slow)*/ printf("%3.3f, %3d, %3.3f, %3.3f, %3d, %3d, %3.3f;\r\n", dist_ir_sensor * 1e3, encoder_M1.read(), speedController_M2.getSpeedRPS(), positionController_M3.getRotation(), servo_position_S1_mus, servo_position_S2_mus, dist_us_sensor); /* read out the imu, the actual frames of the sensor reading needs to be figured out */ /* imu.updateGyro(); imu.updateAcc(); imu.updateMag(); printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n", imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(), imu.readAccX(), imu.readAccY(), imu.readAccZ(), imu.readMagX(), imu.readMagY(), imu.readMagZ()); */ /* ------------- stop hacking ----------------------------------------*/ int loop_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(loop_timer.elapsed_time()).count(); thread_sleep_for(loop_period_ms - loop_time_ms); } } void button_fall() { user_button_timer.reset(); user_button_timer.start(); } void button_rise() { int user_button_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count(); user_button_timer.stop(); if (user_button_time_ms > 200) { do_execute_main = !do_execute_main; } }