Biorobotics 7 / Mbed 2 deprecated PID_example

PID controll for the robot motors.

Dependencies:   BioroboticsMotorControl MODSERIAL mbed

main.cpp@0:7c204101adb0, 2018-10-18 (annotated)

Committer:
brass_phoenix
Date:
Thu Oct 18 12:37:34 2018 +0000
Revision:
0:7c204101adb0
Child:
1:28377623e8c9
P control implemented;

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 0:7c204101adb0 6 const float PI = 3.14159265359;
brass_phoenix 0:7c204101adb0 7 const int PULSES_PER_ROTATION = 6533; // Amount of motor encoder pulses per rotation. When using X4 encoding.
brass_phoenix 0:7c204101adb0 8
brass_phoenix 0:7c204101adb0 9 Ticker motorTicker; // Ticker function
brass_phoenix 0:7c204101adb0 10 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 11 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 12 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 13 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 14 AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 0:7c204101adb0 15 AnalogIn potmeter2(A2); // Analoge input van potmeter 2 -> Motor 2
brass_phoenix 0:7c204101adb0 16 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 17 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 18 Serial pc(USBTX, USBRX);
brass_phoenix 0:7c204101adb0 19
brass_phoenix 0:7c204101adb0 20 // Updates a motor connected to the specified pins with the given speed.
brass_phoenix 0:7c204101adb0 21 // The speed can be both positive and negative.
brass_phoenix 0:7c204101adb0 22 void update_motor(DigitalOut dir, FastPWM pwm, int speed) {
brass_phoenix 0:7c204101adb0 23 // either true or false, determines direction (0 or 1)
brass_phoenix 0:7c204101adb0 24 dir = speed > 0;
brass_phoenix 0:7c204101adb0 25 // 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 0:7c204101adb0 26 pwm = abs(speed);
brass_phoenix 0:7c204101adb0 27 }
brass_phoenix 0:7c204101adb0 28
brass_phoenix 0:7c204101adb0 29 float encoder_pulses_to_radians(int pulses) {
brass_phoenix 0:7c204101adb0 30 return (pulses/(float)PULSES_PER_ROTATION) * 2.0f*PI;
brass_phoenix 0:7c204101adb0 31 }
brass_phoenix 0:7c204101adb0 32
brass_phoenix 0:7c204101adb0 33 // Converts radians/s values into PWM values for motor controll.
brass_phoenix 0:7c204101adb0 34 // Both positive and negative values.
brass_phoenix 0:7c204101adb0 35 int radians_per_second_to_pwm(float rps) {
brass_phoenix 0:7c204101adb0 36 // With our specific motor, full PWM is equal to 1 round per second.
brass_phoenix 0:7c204101adb0 37 // Or 2PI radians per second.
brass_phoenix 0:7c204101adb0 38 int pwm_speed = (rps / (2*PI)) * 255;
brass_phoenix 0:7c204101adb0 39 if (pwm_speed > 255) { pwm_speed = 255; }
brass_phoenix 0:7c204101adb0 40 if (pwm_speed < -255) { pwm_speed = -255; }
brass_phoenix 0:7c204101adb0 41 return pwm_speed;
brass_phoenix 0:7c204101adb0 42 }
brass_phoenix 0:7c204101adb0 43
brass_phoenix 0:7c204101adb0 44 // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
brass_phoenix 0:7c204101adb0 45 float normalize_pot(float pot_value) {
brass_phoenix 0:7c204101adb0 46 // scales value potmeter from 0-1 to -1 - 1.
brass_phoenix 0:7c204101adb0 47 return pot_value * 2 - 1;
brass_phoenix 0:7c204101adb0 48 };
brass_phoenix 0:7c204101adb0 49
brass_phoenix 0:7c204101adb0 50 double P_controller(double error) {
brass_phoenix 0:7c204101adb0 51 double Kp = 10;
brass_phoenix 0:7c204101adb0 52
brass_phoenix 0:7c204101adb0 53 // Proportional part:
brass_phoenix 0:7c204101adb0 54 double u_k = Kp * error;
brass_phoenix 0:7c204101adb0 55
brass_phoenix 0:7c204101adb0 56 return u_k;
brass_phoenix 0:7c204101adb0 57 }
brass_phoenix 0:7c204101adb0 58
brass_phoenix 0:7c204101adb0 59
brass_phoenix 0:7c204101adb0 60 void motorfunction() {
brass_phoenix 0:7c204101adb0 61 // reads out value potmeter 1 between 0-1
brass_phoenix 0:7c204101adb0 62 float pot = potmeter2.read();
brass_phoenix 0:7c204101adb0 63 float desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 0:7c204101adb0 64
brass_phoenix 0:7c204101adb0 65 int pulses = encoder2.getPulses();
brass_phoenix 0:7c204101adb0 66 float current_angle = encoder_pulses_to_radians(pulses);
brass_phoenix 0:7c204101adb0 67
brass_phoenix 0:7c204101adb0 68 float error = current_angle - desired_angle;
brass_phoenix 0:7c204101adb0 69 float speed_rps = P_controller(error);
brass_phoenix 0:7c204101adb0 70
brass_phoenix 0:7c204101adb0 71 int speed_pwm = radians_per_second_to_pwm(speed_rps);
brass_phoenix 0:7c204101adb0 72
brass_phoenix 0:7c204101adb0 73 pc.printf("cur_angle: %f, des_angle: %f, rps: %f, pwm: %i\n", current_angle, desired_angle, speed_rps, speed_pwm);
brass_phoenix 0:7c204101adb0 74
brass_phoenix 0:7c204101adb0 75 update_motor(directionpin2, pwmpin2, speed_pwm);
brass_phoenix 0:7c204101adb0 76 }
brass_phoenix 0:7c204101adb0 77
brass_phoenix 0:7c204101adb0 78
brass_phoenix 0:7c204101adb0 79 int main()
brass_phoenix 0:7c204101adb0 80 {
brass_phoenix 0:7c204101adb0 81 pc.printf("Starting.");
brass_phoenix 0:7c204101adb0 82 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 83 while(true){
brass_phoenix 0:7c204101adb0 84 motorfunction();
brass_phoenix 0:7c204101adb0 85 wait(0.1);
brass_phoenix 0:7c204101adb0 86 } //Lege while loop zodat functie niet afloopt
brass_phoenix 0:7c204101adb0 87 }