Diff: main.cpp

Revision:

37:6ac4db3cc57b

Parent:

36:c6961428c1e1

Child:

38:cbad84e4c714

--- a/main.cpp	Wed Mar 30 11:22:42 2022 +0200
+++ b/main.cpp	Tue Apr 05 08:27:12 2022 +0200
@@ -12,43 +12,56 @@
 #include "PM2_Libary.h"
 
 //------------------------------------general IO and Variables----------------------------------------------------------
+// user button on nucleo board
+Timer user_button_timer;            // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
+InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
+void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
+void user_button_released_fcn();
 
-int ToNextFunction;
+
+//arbitrary allocation of pins -> for testing this has to be adjusted
+AnalogIn ch0(PA_15); //IR TOF sensor at the front
+AnalogIn RearSensor(PC_9); //IR TOF sensor at the back facing down
+
+DigitalOut armMotor(PB_15);
+// main() runs in its own thread in the OS
+// Enable the ADC, 12MHz,  the final term should be (2**BURST_CHANNELS - 1)
 float trigerValueRearSensor = 0.0f;
 float trigerValueFrontSensor = 0.0f;
 
+ int ToNextFunction = 0 ;
+
 
 //-----------------------------------------actors Definition----------------------------------------------------
-// Infrarot sensors pin declaration
-AnalogIn FrontSensor(PC_6); //IR TOF sensor at the front facing down
-AnalogIn RearSensor(PC_8); //IR TOF sensor at the back facing down
-AnalogIn LeftEyeSensor(PC_2); // Infrared sensor on top in "head" part left eye
-AnalogIn RightEyeSensor(PC_3); // Infrared sensor on top in "head" part right eye
+//Encoder pin 
+EncoderCounter  encoder_M_right(PA_0, PA_1);
+EncoderCounter  encoder_M_left(PA_2, PA_3);  //Pin zuweisung Provisorisch!!!
 
-//Encoder pin declaration
-EncoderCounter  encoder_M_right(PA_6, PC_7); //encoder pin decalaration for wheels right side
-EncoderCounter  encoder_M_left(PB_6, PB_7); //encoder pin decalaration for wheels left side
-EncoderCounter encoder_M_arm(PA_0, PA_1); //encoder pin decalaration for arm
-
-//motor pin declaration
-FastPWM pwm_M_right(PA_9);              
-FastPWM pwm_M_left(PB_13);
-FastPWM pwm_M_arm(PA_10);
-
+//motor pin
+FastPWM pwm_M_right(PA_10);              
+FastPWM pwm_M_left(PA_9); 
 // 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_wheels = 2000.0f * 100.0f;    // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
-float counts_per_turn_arm = 40000.0f * 100.0f;      // define counts per turn at gearbox end (counts/turn * gearratio) for arm
+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
 
 //motors for tracks
-PositionController positionController_M_right(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
-PositionController positionController_M_left(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_left, encoder_M_left); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+PositionController positionController_M_right(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+PositionController positionController_M_left(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_left, encoder_M_left); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
 
 //Arm Motor
-PositionController positionController_M_Arm(counts_per_turn_arm * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_arm, encoder_M_arm); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+PositionController positionController_M_Arm(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+
+
+//Platzhalter Variabeln für die Positionierung
+int16_t PositionStair    = 20;
+int16_t PositionBackOff  = 100;
+int16_t degArmStart      = 40;
+int16_t degArmLift       = 18;
+
+
 
 //-----------------------------------------Functions----------------------------------------------------------
 //only moves the arm in to the starting position
@@ -60,25 +73,24 @@
 }
 
 //Drives forward into the next step
-int Drive(int8_t dist){
+int Drive(int16_t dist){
     int8_t i = 0;         //prov condition variable
 
 
     int8_t distance = dist; //distance which will be driven in [mm]
     float factor = 1.0f; //factor for calculating the value in to the float which will be given to the setDesiredRotation function
     float distanceValue = float(distance)*factor;
-    do{
+
         positionController_M_right.setDesiredRotation(distanceValue);
         positionController_M_left.setDesiredRotation(distanceValue);
-        i++;
-    }while(i < 1);
+
     return 0;
     
 }
 
 //only turns the arm until the robot is on the next step
 //not yet clear if the motor controler function drives to a absolute poition or if it drives the given distance relative to the current position
-int LiftUp(int8_t deg){
+int LiftUp(int16_t deg){
     int8_t rotation = deg;
     int8_t i = 0;         //prov condition variable
     do{
@@ -94,20 +106,55 @@
     
 int main()
 {
+    // attach button fall and rise functions to user button object
+    user_button.fall(&user_button_pressed_fcn);
+    user_button.rise(&user_button_released_fcn);
+
+
     while (true) {
-        ToNextFunction = StartPosition();
-        if(NULL != ToNextFunction){
-            ToNextFunction = Drive(3);
-            if(NULL != ToNextFunction){
-                ToNextFunction = LiftUp(5);
-                if(NULL != ToNextFunction){
-                    ToNextFunction = Drive(-1);
+
 
-                }
-            }
+       
+        switch (ToNextFunction) {
+        case 1: StartPosition();
+                ToNextFunction+=1;
+        break;
+        case 2: Drive(PositionStair);
+                ToNextFunction+=1;
+        break;
+        case 3: LiftUp(degArmLift);
+                ToNextFunction+=1;
+        break;
+        case 4: Drive(PositionBackOff);
+                ToNextFunction+=1;
+        break;
+        case 5: LiftUp(degArmStart);
+                ToNextFunction = 0;
+        break;
+        default: ToNextFunction = 0;     
         }
+
         
-thread_sleep_for(10);
+        thread_sleep_for(10);
     }
+
+}
+void user_button_pressed_fcn()
+{
+    user_button_timer.start();
+    user_button_timer.reset();
 }
 
+void user_button_released_fcn()
+{
+    // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
+    int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
+    user_button_timer.stop();
+    if (user_button_elapsed_time_ms > 200) {
+        ToNextFunction = 1;
+    }
+
+
+
+
+}
\ No newline at end of file