Raphael Schmidt / Mbed OS GrannyStepper

Projekt

Dependencies:   PM2_Libary

main.cpp@41:e86f6f2e2bfc, 2022-04-20 (annotated)

Committer:
schmir06
Date:
Wed Apr 20 14:20:47 2022 +0200
Revision:
41:e86f6f2e2bfc
Parent:
40:47997e887235
Child:
42:b3132ace85ab
Servomotor ansteuern

Who changed what in which revision?

UserRevisionLine numberNew contents of line
schmir06 32:2dc6fbd004fd 1 //GrannyStepper
pmic 1:93d997d6b232 2 #include "mbed.h"
pmic 17:c19b471f05cb 3 #include "PM2_Libary.h"
schmir06 34:f035a1869c34 4 #include <iostream>
schmir06 34:f035a1869c34 5 using namespace std;
pmic 6:e1fa1a2d7483 6
schmir06 33:cce9a88a307a 7 //Eingänge
schmir06 34:f035a1869c34 8 DigitalIn user_button(PC_13);
schmir06 40:47997e887235 9 DigitalIn mechanical_button(PC_5);
schmir06 34:f035a1869c34 10
schmir06 33:cce9a88a307a 11 //Ausgänge
schmir06 33:cce9a88a307a 12 DigitalOut enable_motors(PB_15); // Motoren freischalten
pmic 17:c19b471f05cb 13
schmir06 34:f035a1869c34 14 // while loop gets executed every main_task_period_ms milliseconds
schmir06 34:f035a1869c34 15 int main_task_period_ms = 50; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
schmir06 34:f035a1869c34 16 Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
schmir06 34:f035a1869c34 17
schmir06 33:cce9a88a307a 18 //DC-Motoren
schmir06 33:cce9a88a307a 19 float pwm_period_s = 0.00005f; //PWM-Periode fürd DC-Motoren definiert
schmir06 33:cce9a88a307a 20 FastPWM pwm_M1(PB_13);
schmir06 33:cce9a88a307a 21 //Endcoder
schmir06 33:cce9a88a307a 22 EncoderCounter encoder_M1(PA_6, PC_7);
pmic 17:c19b471f05cb 23
pmic 30:1e8295770bc1 24 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
pmic 24:86f1a63e35a0 25 float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
pmic 24:86f1a63e35a0 26 float counts_per_turn = 20.0f * 78.125f; // define counts per turn at gearbox end: counts/turn * gearratio
pmic 25:ea1d6e27c895 27 float kn = 180.0f / 12.0f; // define motor constant in rpm per V
pmic 30:1e8295770bc1 28 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
pmic 30:1e8295770bc1 29 float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1
pmic 6:e1fa1a2d7483 30
pmic 30:1e8295770bc1 31 // SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear box with default contoller parameters
schmir06 40:47997e887235 32 //SpeedController speedController_M1(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M1, encoder_M1); // parameters adjusted to 100:1 gear
schmir06 41:e86f6f2e2bfc 33 //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
pmic 6:e1fa1a2d7483 34
schmir06 40:47997e887235 35 // PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
schmir06 41:e86f6f2e2bfc 36 //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
schmir06 41:e86f6f2e2bfc 37
schmir06 41:e86f6f2e2bfc 38 //Servomotor
schmir06 41:e86f6f2e2bfc 39 Servo servo_S1(PB_12);
schmir06 41:e86f6f2e2bfc 40 float servo_S1_angle = 0;
schmir06 41:e86f6f2e2bfc 41 int servo_period_mus = 20000;
schmir06 41:e86f6f2e2bfc 42
schmir06 41:e86f6f2e2bfc 43 int servo_counter = 0;
schmir06 41:e86f6f2e2bfc 44 int loops_per_seconds = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms)));
schmir06 41:e86f6f2e2bfc 45
pmic 6:e1fa1a2d7483 46
schmir06 34:f035a1869c34 47 int main(){
schmir06 35:57b39290aaea 48
schmir06 35:57b39290aaea 49 int counter_user_button = 0;
schmir06 35:57b39290aaea 50 bool counter_stopp = 0;
schmir06 41:e86f6f2e2bfc 51
schmir06 41:e86f6f2e2bfc 52 pwm_M1.period(pwm_period_s);
schmir06 41:e86f6f2e2bfc 53 pwm_M1.write(0.5f);
schmir06 41:e86f6f2e2bfc 54
schmir06 39:c0c2e33a2181 55
schmir06 36:f678d693969f 56
schmir06 35:57b39290aaea 57 while(1){
schmir06 41:e86f6f2e2bfc 58 //Zähler
schmir06 36:f678d693969f 59 if ((user_button.read() == 0) & (counter_stopp == 0)) {
schmir06 35:57b39290aaea 60 counter_user_button++;
schmir06 35:57b39290aaea 61 counter_stopp = 1;
schmir06 34:f035a1869c34 62
schmir06 35:57b39290aaea 63 }
schmir06 36:f678d693969f 64 if (user_button.read()){
schmir06 35:57b39290aaea 65 counter_stopp = 0;
schmir06 35:57b39290aaea 66 }
schmir06 40:47997e887235 67 if (counter_user_button >= 5){
schmir06 35:57b39290aaea 68 counter_user_button = 1;
schmir06 35:57b39290aaea 69 }
schmir06 41:e86f6f2e2bfc 70
schmir06 41:e86f6f2e2bfc 71 float speed_counter;//Geschwindigkeit für DC-Motor
schmir06 41:e86f6f2e2bfc 72
schmir06 35:57b39290aaea 73 switch (counter_user_button){
schmir06 35:57b39290aaea 74 case 1:
schmir06 41:e86f6f2e2bfc 75 //DC-Motor
schmir06 40:47997e887235 76 enable_motors = 1;
schmir06 41:e86f6f2e2bfc 77 //Servo
schmir06 41:e86f6f2e2bfc 78 servo_S1.Enable(servo_S1_angle, servo_period_mus);
schmir06 35:57b39290aaea 79 break;
schmir06 35:57b39290aaea 80 case 2:
schmir06 41:e86f6f2e2bfc 81 //DC-Motor
schmir06 41:e86f6f2e2bfc 82 speed_counter = 1.0f;
schmir06 41:e86f6f2e2bfc 83 pwm_M1.write(speed_counter);
schmir06 41:e86f6f2e2bfc 84 //Servo
schmir06 41:e86f6f2e2bfc 85 servo_S1.SetPosition(servo_S1_angle);
schmir06 41:e86f6f2e2bfc 86 if (servo_S1_angle <= 0.8f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) {
schmir06 41:e86f6f2e2bfc 87 servo_S1_angle += 0.001f;
schmir06 41:e86f6f2e2bfc 88 }
schmir06 41:e86f6f2e2bfc 89 servo_counter++;
schmir06 39:c0c2e33a2181 90 break;
schmir06 39:c0c2e33a2181 91 case 3:
schmir06 41:e86f6f2e2bfc 92 //DC-Motor
schmir06 41:e86f6f2e2bfc 93 speed_counter = 0.5f;
schmir06 41:e86f6f2e2bfc 94 pwm_M1.write(speed_counter);
schmir06 41:e86f6f2e2bfc 95 //Servo
schmir06 41:e86f6f2e2bfc 96 servo_S1_angle = 1;
schmir06 41:e86f6f2e2bfc 97 servo_S1.SetPosition(servo_S1_angle);
schmir06 40:47997e887235 98 break;
schmir06 40:47997e887235 99 case 4:
schmir06 41:e86f6f2e2bfc 100 //DC-Motor
schmir06 41:e86f6f2e2bfc 101 enable_motors = 0;
schmir06 41:e86f6f2e2bfc 102 //Servo
schmir06 41:e86f6f2e2bfc 103 servo_S1.Disable();
schmir06 41:e86f6f2e2bfc 104 servo_counter = 0;
schmir06 41:e86f6f2e2bfc 105 servo_S1_angle = 0;
schmir06 36:f678d693969f 106 break;
schmir06 35:57b39290aaea 107 }
schmir06 41:e86f6f2e2bfc 108
schmir06 41:e86f6f2e2bfc 109 printf( "counterZähler %d angle %f counterServo %d\n",counter_user_button, servo_S1_angle, servo_counter);
schmir06 39:c0c2e33a2181 110
schmir06 41:e86f6f2e2bfc 111
schmir06 39:c0c2e33a2181 112
schmir06 37:3c427322799d 113 }
schmir06 37:3c427322799d 114
schmir06 36:f678d693969f 115 }
pmic 24:86f1a63e35a0 116
schmir06 34:f035a1869c34 117
schmir06 34:f035a1869c34 118