Biorobotics 7 / Mbed 2 deprecated PID_example

PID controll for the robot motors.

Dependencies:   BioroboticsMotorControl MODSERIAL mbed

main.cpp@1:28377623e8c9, 2018-10-19 (annotated)

Committer:
brass_phoenix
Date:
Fri Oct 19 07:58:26 2018 +0000
Revision:
1:28377623e8c9
Parent:
0:7c204101adb0
Child:
2:3be8cd780b3d
PID implemented. Tuning needed.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brass_phoenix 0:7c204101adb0 1 #include "mbed.h"
brass_phoenix 0:7c204101adb0 2 #include "FastPWM.h"
brass_phoenix 0:7c204101adb0 3 #include "MODSERIAL.h"
brass_phoenix 0:7c204101adb0 4 #include "QEI.h"
brass_phoenix 0:7c204101adb0 5
brass_phoenix 1:28377623e8c9 6 #include "pid.h"
brass_phoenix 1:28377623e8c9 7
brass_phoenix 0:7c204101adb0 8 const float PI = 3.14159265359;
brass_phoenix 0:7c204101adb0 9 const int PULSES_PER_ROTATION = 6533; // Amount of motor encoder pulses per rotation. When using X4 encoding.
brass_phoenix 1:28377623e8c9 10 const float pid_period = 0.1; // PID sample period in seconds.
brass_phoenix 0:7c204101adb0 11
brass_phoenix 1:28377623e8c9 12 const double Kp = 10.0;
brass_phoenix 1:28377623e8c9 13 const double Ki = 0.5;
brass_phoenix 1:28377623e8c9 14 const double Kd = 2.0;
brass_phoenix 1:28377623e8c9 15
brass_phoenix 1:28377623e8c9 16 Ticker pidTicker; // Ticker function
brass_phoenix 0:7c204101adb0 17 FastPWM pwmpin1(D5); // SPECIFIC PIN (hoeft niet aangesloten te worden) Tells you how fast the motor has to go (later: pwmpin.write will tell you the duty cycle, aka how much voltage the motor gets)
brass_phoenix 0:7c204101adb0 18 FastPWM pwmpin2(D6); // SPECIFIC PIN (hoeft niet aangesloten te worden) Tells you how fast the motor has to go (later: pwmpin.write will tell you the duty cycle, aka how much voltage the motor gets)
brass_phoenix 0:7c204101adb0 19 DigitalOut directionpin1(D4); // SPECIFIC PIN (hoeft niet aangesloten te worden) Direction value (0-1) that the mbed will give the motor: in which direction the motor must rotate
brass_phoenix 0:7c204101adb0 20 DigitalOut directionpin2(D7); // SPECIFIC PIN (hoeft niet aangesloten te worden) Direction value (0-1) that the mbed will give the motor: in which direction the motor must rotate
brass_phoenix 0:7c204101adb0 21 AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 0:7c204101adb0 22 AnalogIn potmeter2(A2); // Analoge input van potmeter 2 -> Motor 2
brass_phoenix 0:7c204101adb0 23 QEI encoder1(D11, D10, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 1 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation)
brass_phoenix 0:7c204101adb0 24 QEI encoder2(D13, D12, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 2 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation)
brass_phoenix 0:7c204101adb0 25 Serial pc(USBTX, USBRX);
brass_phoenix 0:7c204101adb0 26
brass_phoenix 1:28377623e8c9 27 PID pid(pid_period);
brass_phoenix 1:28377623e8c9 28
brass_phoenix 0:7c204101adb0 29 // Updates a motor connected to the specified pins with the given speed.
brass_phoenix 1:28377623e8c9 30 // The speed can be both positive and negative, in the range [-1, 1].
brass_phoenix 1:28377623e8c9 31 void update_motor(DigitalOut* dir, FastPWM* pwm, double speed) {
brass_phoenix 1:28377623e8c9 32 if (speed < 1.0 && speed > 0) {
brass_phoenix 1:28377623e8c9 33 // Speed is in the range [0, 1] but the motor only moves
brass_phoenix 1:28377623e8c9 34 // in the range [0.5, 1]. Rescale for this.
brass_phoenix 1:28377623e8c9 35 speed = (speed * 0.5) + 0.5;
brass_phoenix 1:28377623e8c9 36 }
brass_phoenix 1:28377623e8c9 37 if (speed > -1.0 && speed < 0) {
brass_phoenix 1:28377623e8c9 38 // Speed is in the range [-1, 0] but the motor only moves
brass_phoenix 1:28377623e8c9 39 // in the range [-1, -0.5]. Rescale for this.
brass_phoenix 1:28377623e8c9 40 speed = (speed * 0.5) - 0.5;
brass_phoenix 1:28377623e8c9 41 }
brass_phoenix 1:28377623e8c9 42
brass_phoenix 0:7c204101adb0 43 // either true or false, determines direction (0 or 1)
brass_phoenix 1:28377623e8c9 44 *dir = speed > 0;
brass_phoenix 0:7c204101adb0 45 // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value).
brass_phoenix 1:28377623e8c9 46 *pwm = fabs(speed);
brass_phoenix 0:7c204101adb0 47 }
brass_phoenix 0:7c204101adb0 48
brass_phoenix 1:28377623e8c9 49 double encoder_pulses_to_radians(int pulses) {
brass_phoenix 1:28377623e8c9 50 return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI;
brass_phoenix 0:7c204101adb0 51 }
brass_phoenix 0:7c204101adb0 52
brass_phoenix 0:7c204101adb0 53 // Converts radians/s values into PWM values for motor controll.
brass_phoenix 0:7c204101adb0 54 // Both positive and negative values.
brass_phoenix 1:28377623e8c9 55 double radians_per_second_to_pwm(double rps) {
brass_phoenix 0:7c204101adb0 56 // With our specific motor, full PWM is equal to 1 round per second.
brass_phoenix 0:7c204101adb0 57 // Or 2PI radians per second.
brass_phoenix 1:28377623e8c9 58 double pwm_speed = rps / (2*PI);
brass_phoenix 1:28377623e8c9 59
brass_phoenix 1:28377623e8c9 60 if (pwm_speed > 1) { pwm_speed = 1; }
brass_phoenix 1:28377623e8c9 61 if (pwm_speed < -1) { pwm_speed = -1; }
brass_phoenix 0:7c204101adb0 62 return pwm_speed;
brass_phoenix 0:7c204101adb0 63 }
brass_phoenix 0:7c204101adb0 64
brass_phoenix 0:7c204101adb0 65 // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
brass_phoenix 1:28377623e8c9 66 double normalize_pot(double pot_value) {
brass_phoenix 0:7c204101adb0 67 // scales value potmeter from 0-1 to -1 - 1.
brass_phoenix 0:7c204101adb0 68 return pot_value * 2 - 1;
brass_phoenix 0:7c204101adb0 69 };
brass_phoenix 0:7c204101adb0 70
brass_phoenix 0:7c204101adb0 71
brass_phoenix 0:7c204101adb0 72 void motorfunction() {
brass_phoenix 0:7c204101adb0 73 // reads out value potmeter 1 between 0-1
brass_phoenix 1:28377623e8c9 74 double pot = potmeter2.read();
brass_phoenix 1:28377623e8c9 75 double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 0:7c204101adb0 76
brass_phoenix 0:7c204101adb0 77 int pulses = encoder2.getPulses();
brass_phoenix 1:28377623e8c9 78 double current_angle = encoder_pulses_to_radians(pulses);
brass_phoenix 0:7c204101adb0 79
brass_phoenix 1:28377623e8c9 80 double error = current_angle - desired_angle;
brass_phoenix 1:28377623e8c9 81 // PID controll.
brass_phoenix 1:28377623e8c9 82 double speed_rps = pid.update(error);
brass_phoenix 0:7c204101adb0 83
brass_phoenix 1:28377623e8c9 84 double speed_pwm = radians_per_second_to_pwm(speed_rps);
brass_phoenix 0:7c204101adb0 85
brass_phoenix 1:28377623e8c9 86 pc.printf("puls: %i, c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", pulses, current_angle, desired_angle, error, speed_rps, speed_pwm);
brass_phoenix 0:7c204101adb0 87
brass_phoenix 1:28377623e8c9 88 update_motor(&directionpin2, &pwmpin2, speed_pwm);
brass_phoenix 0:7c204101adb0 89 }
brass_phoenix 0:7c204101adb0 90
brass_phoenix 0:7c204101adb0 91
brass_phoenix 0:7c204101adb0 92 int main()
brass_phoenix 0:7c204101adb0 93 {
brass_phoenix 1:28377623e8c9 94 pc.baud(115200);
brass_phoenix 0:7c204101adb0 95 pc.printf("Starting.");
brass_phoenix 1:28377623e8c9 96 pid.set_k_values(Kp, Ki, Kd);
brass_phoenix 1:28377623e8c9 97 pidTicker.attach(motorfunction, pid_period);
brass_phoenix 0:7c204101adb0 98 pwmpin1.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once)
brass_phoenix 0:7c204101adb0 99 while(true){
brass_phoenix 0:7c204101adb0 100 } //Lege while loop zodat functie niet afloopt
brass_phoenix 0:7c204101adb0 101 }