Biorobotics 7 / Mbed 2 deprecated PID_example

PID controll for the robot motors.

Dependencies:   BioroboticsMotorControl MODSERIAL mbed

main.cpp@7:12fedc93c6ad, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 09:12:59 2018 +0000
Revision:
7:12fedc93c6ad
Parent:
6:cc3e47c7aac2 
+ Added reduction ratios.

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 "MODSERIAL.h"
brass_phoenix 0:7c204101adb0 3
brass_phoenix 5:fd4eaf3cb09d 4 #include "motor.h"
brass_phoenix 1:28377623e8c9 5
brass_phoenix 4:fce8c7458e97 6 const float pid_period = 0.001; // PID sample period in seconds.
brass_phoenix 0:7c204101adb0 7
brass_phoenix 3:689f3f2e78e8 8 const double Kp = 10.0;
brass_phoenix 4:fce8c7458e97 9 const double Ki = 0.1;
brass_phoenix 3:689f3f2e78e8 10 const double Kd = 0.5;
brass_phoenix 2:3be8cd780b3d 11
brass_phoenix 7:12fedc93c6ad 12 AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 7:12fedc93c6ad 13 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2
brass_phoenix 0:7c204101adb0 14 Serial pc(USBTX, USBRX);
brass_phoenix 0:7c204101adb0 15
brass_phoenix 6:cc3e47c7aac2 16 Motor motor1(D6, D7, D13, D12, &pc);
brass_phoenix 6:cc3e47c7aac2 17 Motor motor2(D5, D4, D10, D11, &pc);
brass_phoenix 0:7c204101adb0 18
brass_phoenix 0:7c204101adb0 19
brass_phoenix 7:12fedc93c6ad 20 const double PI = 3.14159265359;
brass_phoenix 7:12fedc93c6ad 21
brass_phoenix 0:7c204101adb0 22 // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
brass_phoenix 1:28377623e8c9 23 double normalize_pot(double pot_value) {
brass_phoenix 0:7c204101adb0 24 // scales value potmeter from 0-1 to -1 - 1.
brass_phoenix 0:7c204101adb0 25 return pot_value * 2 - 1;
brass_phoenix 0:7c204101adb0 26 };
brass_phoenix 0:7c204101adb0 27
brass_phoenix 0:7c204101adb0 28
brass_phoenix 0:7c204101adb0 29 int main()
brass_phoenix 0:7c204101adb0 30 {
brass_phoenix 1:28377623e8c9 31 pc.baud(115200);
brass_phoenix 0:7c204101adb0 32 pc.printf("Starting.");
brass_phoenix 6:cc3e47c7aac2 33 motor1.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 6:cc3e47c7aac2 34 motor2.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 7:12fedc93c6ad 35
brass_phoenix 7:12fedc93c6ad 36 motor1.set_extra_reduction_ratio(-0.5);
brass_phoenix 7:12fedc93c6ad 37 motor2.set_extra_reduction_ratio(0.5);
brass_phoenix 7:12fedc93c6ad 38
brass_phoenix 5:fd4eaf3cb09d 39 // Start the motor controller at the desired frequency.
brass_phoenix 6:cc3e47c7aac2 40 motor1.start(pid_period);
brass_phoenix 6:cc3e47c7aac2 41 motor2.start(pid_period);
brass_phoenix 5:fd4eaf3cb09d 42
brass_phoenix 0:7c204101adb0 43 while(true){
brass_phoenix 5:fd4eaf3cb09d 44 // reads out value potmeter 1 between 0-1
brass_phoenix 6:cc3e47c7aac2 45 double pot = potmeter1.read();
brass_phoenix 5:fd4eaf3cb09d 46 double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 5:fd4eaf3cb09d 47 // Update the motor controller with the new angle.
brass_phoenix 6:cc3e47c7aac2 48 motor1.set_target_angle(desired_angle);
brass_phoenix 6:cc3e47c7aac2 49
brass_phoenix 6:cc3e47c7aac2 50 // reads out value potmeter 1 between 0-1
brass_phoenix 6:cc3e47c7aac2 51 pot = potmeter2.read();
brass_phoenix 6:cc3e47c7aac2 52 desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 6:cc3e47c7aac2 53 // Update the motor controller with the new angle.
brass_phoenix 6:cc3e47c7aac2 54 motor2.set_target_angle(desired_angle);
brass_phoenix 6:cc3e47c7aac2 55
brass_phoenix 5:fd4eaf3cb09d 56 wait(0.1);
brass_phoenix 0:7c204101adb0 57 } //Lege while loop zodat functie niet afloopt
brass_phoenix 0:7c204101adb0 58 }