Diff: main.cpp

Revision:

41:e86f6f2e2bfc

Parent:

40:47997e887235

Child:

42:b3132ace85ab

--- a/main.cpp	Wed Apr 20 08:22:36 2022 +0200
+++ b/main.cpp	Wed Apr 20 14:20:47 2022 +0200
@@ -30,20 +30,32 @@
 
 // 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_M1(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear
-float max_speed_rps = 0.5f;                 // 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 = 0.5f;                 // define maximum speed that the position controller is changig the speed, has to be smaller or equal to kn * max_voltage
 
 // PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
-PositionController positionController_M1(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+//PositionController positionController_M1(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+
+//Servomotor
+Servo servo_S1(PB_12);
+float servo_S1_angle = 0; 
+int servo_period_mus = 20000;
+
+int servo_counter = 0; 
+int loops_per_seconds = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms)));
+
 
 int main(){
 
-
     int counter_user_button = 0;
     bool counter_stopp = 0;
+
+    pwm_M1.period(pwm_period_s);
+    pwm_M1.write(0.5f);
+    
     
 
     while(1){
-        
+        //Zähler
         if ((user_button.read() == 0) & (counter_stopp == 0)) {
             counter_user_button++;
             counter_stopp = 1;
@@ -55,39 +67,48 @@
         if (counter_user_button >= 5){
             counter_user_button = 1;
         }
-        float speed_counter;
+
+        float speed_counter;//Geschwindigkeit für DC-Motor
+
         switch (counter_user_button){
             case 1:
+            //DC-Motor
             enable_motors = 1;
-            speed_counter = 0.0f;
-            positionController_M1.setDesiredRotation(0.0f, 0.0f);
+            //Servo
+            servo_S1.Enable(servo_S1_angle, servo_period_mus);
             break;
             case 2:
-            enable_motors = 1;
-            speed_counter = 0.1f;
-            positionController_M1.setDesiredRotation(1.5f, 0.2f);
+            //DC-Motor
+            speed_counter = 1.0f;
+            pwm_M1.write(speed_counter);
+            //Servo
+            servo_S1.SetPosition(servo_S1_angle);
+            if (servo_S1_angle <= 0.8f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) {
+                servo_S1_angle += 0.001f;
+            }
+            servo_counter++;
             break; 
             case 3:
-            enable_motors = 1;
-            speed_counter = 0.0f;
-            positionController_M1.setDesiredRotation(0.0f, 0.0f);
+            //DC-Motor
+            speed_counter = 0.5f;
+            pwm_M1.write(speed_counter);
+            //Servo
+            servo_S1_angle = 1;
+            servo_S1.SetPosition(servo_S1_angle);
             break; 
             case 4:
-            enable_motors = 1;
-            speed_counter = 1.0f;
-            positionController_M1.setDesiredRotation(1.5f, 1.0f);
+            //DC-Motor
+            enable_motors = 0;
+            //Servo
+            servo_S1.Disable();
+            servo_counter = 0;
+            servo_S1_angle = 0;
             break; 
         }
+    
+        printf( "counterZähler %d   angle %f    counterServo  %d\n",counter_user_button, servo_S1_angle, servo_counter);
 
-        //positionController_M1.setDesiredRotation(1.5f, speed_counter);
-    
-        /*if (counter_user_button == 1 || counter_user_button == 3 || counter_user_button == 5){
-        speedController_M1.setDesiredSpeedRPS(speed_counter);
-        //pwm_M1.write(speed_counter);
-        counter_user_button++;
-        }*/
-    
-        printf( "counter %d   speed %f \n",counter_user_button, speed_counter);
+        
         
     }
         
@@ -95,6 +116,3 @@
 
 
 
-
-
-