Diff: main.cpp

Revision:

38:6d11788e14c0

Parent:

34:702246639f02

Child:

40:eb7f8dce5787

--- a/main.cpp	Thu May 05 09:16:39 2022 +0000
+++ b/main.cpp	Thu May 05 18:01:37 2022 +0200
@@ -21,9 +21,43 @@
 // led on nucleo board
 DigitalOut user_led(LED1);      // create DigitalOut object to command user led
 
+// Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
+float ir_distance_mV = 0.0f;    // define variable to store measurement
+AnalogIn ir_analog_in(PC_2);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
+
+// 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
+DigitalOut enable_motors(PB_15);    // create DigitalOut object to enable dc motors
+
+float   pwm_period_s = 0.00005f;    // define pwm period time in seconds and create FastPWM objects to command dc motors
+FastPWM pwm_M1(PB_13);              // motor M1 is closed-loop speed controlled (angle velocity)
+FastPWM pwm_M2(PA_9);               // motor M2 is closed-loop speed controlled (angle velocity)
+
+EncoderCounter  encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
+EncoderCounter  encoder_M2(PB_6, PB_7);
+
+// create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
+float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
+float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
+float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
+float k_gear = 100.0f / 78.125f;            // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1
+float kp = 0.1f;                            // define custom kp, this is the default speed controller gain for gear box 78.125:1
+
+SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1); // default 78.125:1 gear box  with default contoller parameters
+SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear box  with default contoller parameters
+// SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 100:1 gear
+
+// sparkfun line follower array
 I2C i2c(PB_9, PB_8); // I2C (PinName sda, PinName scl)
 SensorBar sensor_bar(i2c, 0.1175f);
 
+float r_wheel = 0.0358f / 2.0f;
+float L_wheel = 0.143f;
+// transform wheel to robot
+float Crw[2][2] = {{r_wheel / 2.0f, r_wheel / 2.0f}, {r_wheel / L_wheel, -r_wheel / L_wheel}};
+// transform robot to wheel
+float Cwr[2][2] = {{1.0f / r_wheel, L_wheel / (2.0f * r_wheel)}, {1.0f / r_wheel, -L_wheel / (2.0f * r_wheel)}};
+// float Test = Crw[0][]
+
 int main()
 {
     // attach button fall and rise functions to user button object
@@ -33,10 +67,8 @@
     // start timer
     main_task_timer.start();
 
-    // sensor_bar.setBarStrobe();
-    // sensor_bar.clearBarStrobe();  // to illuminate all the time
-    // sensor_bar.clearInvertBits(); // to make the bar look for a dark line on a reflective surface
-    // sensor_bar.begin();
+    // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled
+    enable_motors = 1;
 
     while (true) { // this loop will run forever
 
@@ -44,6 +76,12 @@
 
         if (do_execute_main_task) {
 
+            // read analog input
+            ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
+
+            speedController_M1.setDesiredSpeedRPS(0.5f); // set a desired speed for speed controlled dc motors M2
+            speedController_M2.setDesiredSpeedRPS(0.5f); // set a desired speed for speed controlled dc motors M2
+
             /*
             uint8_t sensor_bar_raw_value = sensor_bar.getRaw();
             for( int i = 7; i >= 0; i-- ) {
@@ -69,6 +107,10 @@
 
         } else {
 
+            ir_distance_mV = 0.0f;
+
+            speedController_M1.setDesiredSpeedRPS(0.0f);
+            speedController_M2.setDesiredSpeedRPS(0.0f);
         }
 
         user_led = !user_led;