Revision 46:cb94c71ac0ff, committed 2022-05-23

Comitter:

okubo012

Date:

Mon May 23 11:50:59 2022 +0200

Parent:

45:ea155b2ad84d

Commit message:

Added libs

Changed in this revision

--- a/main.cpp	Wed May 18 06:52:25 2022 +0000
+++ b/main.cpp	Mon May 23 11:50:59 2022 +0200
@@ -3,9 +3,9 @@
 #include "PM2_Libary.h"
 #include "Eigen/Dense.h"
 
-// workshop 1
+# define M_PI 3.14159265358979323846  // number pi
 
-# define M_PI 3.14159265358979323846  // number pi
+// Workshop 1
 
 // logical variable main task
 bool do_execute_main_task = false;  // this variable will be toggled via the user button (blue button) to or not to execute the main task
@@ -16,10 +16,12 @@
 void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
 void user_button_released_fcn();
 
+float ir_distance_mV_to_cm(float mV);
+
 int main()
 {
     // while loop gets executed every main_task_period_ms milliseconds
-    const int main_task_period_ms = 10;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+    const int main_task_period_ms = 100;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
     Timer main_task_timer;                // create Timer object which we use to run the main task every main task period time in ms
 
     // a coutner
@@ -28,6 +30,53 @@
     // led on nucleo board
     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
 
+    // IR init
+    AnalogIn ir_analog_in(PC_2);
+    float ir_distance_mV = 0.0f;
+    float ir_distance_cm = 0.0f;
+
+    // Line sensor bar init
+    // I2C i2c(PB_9, PB_8);
+    // SensorBar sensorBar(i2c, 0.0f); // .1f = 10 cm away from center of driving axis
+
+    // DC motor init
+    // 0 to 1 maps to -12V and 12V, so 0 to .5 is backwards and .5 to 1 is forwards
+    // 1 is fastest forward and 0 is fastest backwards
+    DigitalOut enable_motors(PB_2);
+    enable_motors = 1;
+
+    // Motors
+    FastPWM pwm_M1(PA_8);
+    FastPWM pwm_M2(PA_9);    
+
+    // Encoders
+    EncoderCounter  encoder_M1(PB_6, PB_7);
+    EncoderCounter  encoder_M2(PA_6, PC_7);
+
+    // // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
+    // const float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
+    // const float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
+    // const float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
+    // //const 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
+    // //const 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_M1(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear
+    // speedController_M1.setMaxAccelerationRPS(999.0f);  // disable internal trajectory planer
+ 
+    // PositionController positionController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear with default contoller parameters
+    // //PositionController positionController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+    // //positionController_M2.setSpeedCntrlGain(kp * k_gear);
+    // positionController_M2.setMaxAccelerationRPS(999.0f);  // disable internal trajectory planer
+    // // define maximum speed that the position controller is changig the speed, has to be smaller or equal to kn * max_voltage
+    // float max_speed_rps = 2.0f;
+    // positionController_M2.setMaxVelocityRPS(max_speed_rps);
+
+    const float pwm_period_s = .00005f;
+    pwm_M1.period(pwm_period_s);
+    pwm_M2.period(pwm_period_s);
+
+
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
@@ -39,11 +88,15 @@
 
         main_task_timer.reset();
         
-       
-        if (do_execute_main_task) {
+        // Get digital value of IR sensor and convert to analog
+        ir_distance_mV = ir_analog_in.read() * 3.3f * 1.0e3f;
 
+        if (do_execute_main_task) {
+            pwm_M1.write(0.75f);
+            pwm_M2.write(0.75f);
         } else {
-            
+            pwm_M1.write(0.5f);
+            pwm_M2.write(0.5f);
         }
 
         // user_led is switching its state every second
@@ -51,7 +104,17 @@
             user_led = !user_led;
         }
         main_task_cntr++;
+
+        // IR sensor printing
+        // printf("IR Sensor (mV): %f (cm): %f \r\n", ir_distance_mV, ir_distance_mV_to_cm(ir_distance_mV));
+
+        // Line following
+        // if (sensorBar.isAnyLedActive()){
+        //     printf("Avg Angle (rad): %f \n", (sensorBar.getAvgAngleRad() * (180.0f/M_PI)));
+        // }
         
+        printf("Encoder Val: %f \r\n", encoder_M1.read());
+
         // do only output via serial what's really necessary (this makes your code slow)
         /*
         printf("IR sensor (mV): %3.3f, IR sensor (cm): %3.3f, SensorBar angle (rad): %3.3f, Speed M1 (rps) %3.3f, Position M2 (rot): %3.3f\r\n",
@@ -82,4 +145,10 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
+}
+
+float ir_distance_mV_to_cm(float mV) {
+    static float a = -3.952f;
+    static float c = 1.663e+04f;
+    return c/(mV+1) + a;
 }
\ No newline at end of file