Workshop 1
Dependencies: PM2_Libary Eigen
main.cpp
- Committer:
- pmic
- Date:
- 2021-04-19
- Revision:
- 12:3dfd8f2939ac
- Parent:
- 11:af0f165f8761
- Child:
- 13:41bf923cd055
File content as of revision 12:3dfd8f2939ac:
#include "mbed.h" #include "platform/mbed_thread.h" /* PM2_Libary */ #include "EncoderCounter.h" #include "Servo.h" #include "SpeedController.h" #include "FastPWM.h" #include "RangeFinder.h" using namespace std::chrono; InterruptIn user_button(USER_BUTTON); DigitalOut led(LED1); bool executeMainTask = false; Timer user_button_timer, loop_timer; int Ts_ms = 50; /* declaration of custom button functions */ void button_fall(); void button_rise(); // SHARP GP2Y0A21Y IR Sensor // https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds /* create analog input object */ AnalogIn analogIn(PC_2); float dist_IRSensor = 0.0f; // 78:1 Metal Gearmotor 20Dx43L mm 12V CB // https://www.pololu.com/product/3477 /* create enable dc motor digital out object */ DigitalOut enable_motors(PB_15); /* create pwm objects */ FastPWM pwmOut_M1(PB_13); FastPWM pwmOut_M2(PA_9); FastPWM pwmOut_M3(PA_10); double Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end) /* create encoder read objects */ EncoderCounter encoderCounter_M1(PA_6, PC_7); EncoderCounter encoderCounter_M2(PB_6, PB_7); EncoderCounter encoderCounter_M3(PA_0, PA_1); /* create speed controller objects, only M1 and M2, M3 is used open-loop */ float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio float kn = 180.0f/12.0f; // (RPM/V) float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1); SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2); // Futaba Servo S3001 20mm 3kg Analog // 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 /* 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 Ts_pwm_mus = 20000; int servoOutput_mus_S1 = 0; int servoOutput_mus_S2 = 0; int servo_counter = 0; int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_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 rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization // RangeFinder rangefinder(PB_6, 5782.0f, 0.02f, 7000); // 1/Ts_ms = 50 Hz parametrization float dist_USSensor = 0.0f; int main() { user_button.fall(&button_fall); user_button.rise(&button_rise); loop_timer.start(); /* enable hardwaredriver dc motors */ enable_motors = 1; /* initialize pwm for motor M3*/ pwmOut_M3.period(Ts_pwm_s); /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */ pwmOut_M3.write(0.5); /* enable servos, you can also disable them */ servo_S1.Enable(servoOutput_mus_S1, Ts_pwm_mus); servo_S2.Enable(servoOutput_mus_S2, Ts_pwm_mus); while (true) { loop_timer.reset(); /* ------------- start hacking ------------- -------------*/ if (executeMainTask) { /* read analog input */ dist_IRSensor = analogIn.read() * 3.3f; /* command a speed to dc motors M1 and M2*/ speedController_M1.setDesiredSpeedRPS( 1.0f); speedController_M2.setDesiredSpeedRPS(-0.5f); /* write output voltage to motor M3 */ pwmOut_M3.write(0.75); /* command servo position via output time, this needs to be calibrated */ servo_S1.SetPosition(servoOutput_mus_S1); servo_S2.SetPosition(servoOutput_mus_S2); if (servoOutput_mus_S1 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) { servoOutput_mus_S1 += 100; } if (servoOutput_mus_S2 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) { servoOutput_mus_S2 += 100; } servo_counter++; /* read ultra sonic distance sensor */ dist_USSensor = rangeFinder.read_cm(); /* visual feedback that the main task is executed */ led = !led; } else { dist_IRSensor = 0.0f; speedController_M1.setDesiredSpeedRPS(0.0f); speedController_M2.setDesiredSpeedRPS(0.0f); pwmOut_M3.write(0.5); servoOutput_mus_S1 = 0; servoOutput_mus_S2 = 0; servo_S1.SetPosition(servoOutput_mus_S1); servo_S2.SetPosition(servoOutput_mus_S2); dist_USSensor = 0.0f; led = 0; } /* do only output via serial what's really necessary (this makes your code slow)*/ printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n", dist_IRSensor, servoOutput_mus_S1, servoOutput_mus_S2, encoderCounter_M3.read(), speedController_M1.getSpeedRPS(), speedController_M2.getSpeedRPS(), dist_USSensor); /* ------------- stop hacking ------------- -------------*/ int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count(); int dT_loop_ms = Ts_ms - T_loop_ms; thread_sleep_for(dT_loop_ms); } } void button_fall() { user_button_timer.reset(); user_button_timer.start(); } void button_rise() { int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count(); user_button_timer.stop(); if (t_button_ms > 200) { executeMainTask = !executeMainTask; } }