Diff: main.cpp

Revision:

34:9f779e91168e

Parent:

33:70ea029a69e8

Child:

35:f02adb2c2b8a

Child:

38:c2663f7dcccb

Child:

39:025d1bee1397

--- a/main.cpp	Wed Apr 06 12:38:20 2022 +0200
+++ b/main.cpp	Sun Apr 10 19:40:59 2022 +0200
@@ -12,7 +12,7 @@
 void user_button_released_fcn();
 
 // while loop gets executed every main_task_period_ms milliseconds
-int main_task_period_ms = 50;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+int main_task_period_ms = 30;   // 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
 
 // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
@@ -26,8 +26,8 @@
 
 float   pwm_period_s = 0.00005f;    // define pwm period time in seconds and create FastPWM objects to command dc motors
 //motor pin declaration
-FastPWM pwm_M_right(PA_9);              
-FastPWM pwm_M_left(PB_13);
+FastPWM pwm_M_right(PB_13);              
+FastPWM pwm_M_left(PA_9);
 FastPWM pwm_M_arm(PA_10);
 
 //Encoder pin declaration
@@ -38,7 +38,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_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_arm = 2000.0f * 100.0f;      // define counts per turn at gearbox end (counts/turn * gearratio) for arm
 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
@@ -58,36 +58,38 @@
 // LSM9DS1 imu(PC_9, PA_8); // create LSM9DS1 comunication object, if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h"
 
 //Platzhalter Variabeln für die Positionierung
-int16_t PositionStair    = 20;
-int16_t PositionBackOff  = 100;
-int16_t degArmStart      = 40;
-int16_t degArmLift       = 18;
-int ToNextFunction = 1;
+float PositionStair    = 0.2;
+float PositionBackOff  = -0.5;
+float degArmStart      = 0.5;
+float degArmLift       = -0.5;
+int ToNextFunction = 0;
+float max_speed_rps = 0.5f;  
 
-int StartPosition(int16_t deg){
+int StartPosition(float deg){
 
     positionController_M_Arm.setDesiredRotation(deg);
 
     return NULL;
 }
 //Drives forward into the next step
-int Drive(int16_t dist){
-    int8_t i = 0;         //prov condition variable
+int Drive(float dist){
+
+float distance;
 
-    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;
+distance=dist;
+
 
-        positionController_M_right.setDesiredRotation(distanceValue);
-        positionController_M_left.setDesiredRotation(distanceValue);
+    positionController_M_right.setDesiredRotation(distance,max_speed_rps);
+    positionController_M_left.setDesiredRotation(distance,max_speed_rps);
+
 
     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(int16_t deg){
-    int8_t rotation = deg;
+int LiftUp(float deg){
+
     int8_t i = 0;         //prov condition variable
     
         positionController_M_Arm.setDesiredRotation(deg);
@@ -133,40 +135,45 @@
 {
     // attach button fall and rise functions to user button object
 user_button.fall(&user_button_pressed_fcn);
-   user_button.rise(&user_button_released_fcn);
+user_button.rise(&user_button_released_fcn);
   
 
 
     while (true){
+         enable_motors = 1;
+
          ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
         
-         printf("test pow function 2 ^ 2 %lf\n",powerx(2,2));
-         printf("test mapping function %f\n", mapping(ir_distance_mV));
+        // printf("test pow function 2 ^ 2 %lf\n",powerx(2,2));
+         //printf("test mapping function %f\n", mapping(ir_distance_mV));
 
-         printf("IR sensor (mV): %3.3f\n", ir_distance_mV);
+         //printf("IR sensor (mV): %3.3f\n", ir_distance_mV);
         
 
        switch (ToNextFunction) {
         case 1: StartPosition(degArmStart);
-            printf("a");
+            printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
              //   ToNextFunction+=1;
             break;
         case 2: Drive(PositionStair);
-            printf("b");
+           printf("Case 2: Position Right(rot): %3.3f;    Position Left (rot): %3.3f\n",
+           positionController_M_right.getRotation(),positionController_M_left.getRotation());
             // ToNextFunction+=1;
             break;
-        case 3:// LiftUp(degArmLift);
+        case 3: LiftUp(degArmLift);
             //  ToNextFunction+=1;
-            printf("c");
+            printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
              break;
         case 4: Drive(PositionBackOff);
-            printf("d");
+            printf("Case 4: Position Right(rot): %3.3f;    Position Left (rot): %3.3f\n",
+           positionController_M_right.getRotation(),positionController_M_left.getRotation());
             //   ToNextFunction+=1;
             break;
         case 5: LiftUp(degArmStart);
-                printf("d");
+                printf("Case 5: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
             //  ToNextFunction = 0;
             break;  
+            default:  ;
         }