Diff: main.cpp

Revision:

33:479abfdf8af8

Parent:

32:700882b2140e

--- a/main.cpp	Wed Mar 30 10:23:08 2022 +0200
+++ b/main.cpp	Wed May 11 14:51:43 2022 +0200
@@ -1,16 +1,30 @@
+//workshop 2
 #include "mbed.h"
 #include "PM2_Libary.h"
+#include <cstdio>
 
-//Eingänge
-InterruptIn user_button(PC_13); //Blauer Taster für Startsignal
-//Ausgänge
-DigitalOut enable_motors(PB_15); // Motoren freischalten
+// mechanical button
+DigitalIn mechanical_button(PC_5);
+
+// 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)
 
-//DC-Motoren
-float   pwm_period_s = 0.00005f; //PWM-Periode fürd DC-Motoren definiert
-FastPWM pwm_M1(PB_13); 
-//Endcoder
-EncoderCounter  encoder_M1(PA_6, PC_7);
+// 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
+Timer main_task_timer;          // create Timer object which we use to run the main task every main task period time in ms
+
+// 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_Move(PA_10);              // motor M1 is used open loop
+FastPWM pwm_FrontSpindle(PA_9);               // motor M2 is closed-loop speed controlled (angle velocity)
+FastPWM pwm_BackSpindle(PB_13);              // motor M3 is closed-loop position controlled (angle controlled)
+
+EncoderCounter  encoder_Move(PA_0, PA_1); // create encoder objects to read in the encoder counter values
+EncoderCounter  encoder_FrontSpindle(PB_6, PB_7);
+EncoderCounter  encoder_BackSpindle(PA_6, PC_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
@@ -19,10 +33,114 @@
 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_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_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
+// PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
+PositionController positionController_Move(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_Move, encoder_Move); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+
+float speed_spindle = 0.5f;
+
+
+DigitalIn endswitch_step(PC_6);         // define digital input endswitch step pin PC_6
+DigitalIn endswitch_spindle_f(PB_12);    // define digital input endswitch spindle front pin PB_2 
+DigitalIn endswitch_spindle_b(PC_8);     // define digital input endswitch spindle back pin PC_8
+
+bool end_step(){                        //function to check if next step is reached
+    if(endswitch_step.read()){             //define pullup mode!!
+    return false;
+    }else{
+        return true;
+    }
+}
+
+bool end_spindel_f(){                   //function to check if front spindle is in position 0
+    if(endswitch_spindle_f.read()){
+        return true;
+    }else{
+        return false;
+    }
+}
+
+bool end_spindel_b(){                   //function to check if back spindle is in position 0
+    if(endswitch_spindle_b.read()){
+        return true;
+    }else{
+        return false;
+    }
+}
+
+
+int main(){
+    int counter_stoppen;
+    int counter_user_button = 0;
+    int positionStep;
+    float actualPos;
+    int counter_actualPos;
+    float move2;
+    float stoppen2;
+    endswitch_step.mode(PullDown);
+    endswitch_spindle_f.mode(PullDown);
+    endswitch_spindle_b.mode(PullDown);
+    // start timer
+    main_task_timer.start();
+
+    // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled
+    enable_motors = 1;
+
+    // motor M1 is used open-loop, we need to initialize the pwm and set pwm output to zero at the beginning, range: 0...1 -> u_min...u_max: 0.5 -> 0 V
+    pwm_BackSpindle.period(pwm_period_s);
+    pwm_BackSpindle.write(0.5f);
+
+    pwm_FrontSpindle.period(pwm_period_s);
+    pwm_FrontSpindle.write(0.5f);    
+
+
+    while (true) { // this loop will run forever
+    if ((user_button.read() == 0) & (counter_stoppen == 0)) {
+        counter_user_button++;
+        counter_stoppen = 1;
+    }
+	if (user_button.read()){
+        counter_stoppen = 0;
+    }
+
+    if (counter_user_button >= 4){
+        counter_user_button = 1;
+    }
+
+
+    switch (counter_user_button){
+        case 1:
+            counter_actualPos = 0;
+            positionController_Move.setDesiredRotation(-3.0f, max_speed_rps);
+            positionStep = 1;
+            printf("aktuelle Rotation:%3.3f,  Schritt %d,  Endschalter %x\n", positionController_Move.getRotation(), positionStep, endswitch_step.read());
+            if(end_step()){
+                counter_user_button++;
+            }
+        break;
+        case 2:
+            stoppen2 = positionController_Move.getRotation();
+            positionController_Move.setDesiredRotation(stoppen2, max_speed_rps);
+            positionStep = 2;
+            printf("aktuelle Rotation:%3.3f,  Schritt %d,  SB %x\n", positionController_Move.getRotation(), positionStep, endswitch_spindle_b.read());
+        break;
+        case 3:
+            if((counter_user_button == 3) && (counter_actualPos == 0)){
+                actualPos =  positionController_Move.getRotation();
+                counter_actualPos = 1;
+            }
+            move2 = actualPos+1.0f;
+            positionController_Move.setDesiredRotation(move2, max_speed_rps);
+            positionStep = 3;
+            printf("aktuelle Rotation:%3.3f,  Schritt %d,  move2 %f,  SF %x\n", positionController_Move.getRotation(), positionStep, move2, endswitch_spindle_f.read());
+        break;
+
+
+                // 0.75 ist einfahren und rückwärtsfahren
+                // 0.25 ist ausfahren und vorwärtsfahren
+        }
+    }
+}
 
 
 
-