Biorobotics 7 / Mbed 2 deprecated PID_example

PID controll for the robot motors.

Dependencies:   BioroboticsMotorControl MODSERIAL mbed

main.cpp@6:cc3e47c7aac2, 2018-10-19 (annotated)

Committer:
brass_phoenix
Date:
Fri Oct 19 11:21:40 2018 +0000
Revision:
6:cc3e47c7aac2
Parent:
5:fd4eaf3cb09d
Child:
7:12fedc93c6ad
+ Now with 2 motors.

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 0:7c204101adb0 12 AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 0:7c204101adb0 13 AnalogIn potmeter2(A2); // 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 0:7c204101adb0 20 // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
brass_phoenix 1:28377623e8c9 21 double normalize_pot(double pot_value) {
brass_phoenix 0:7c204101adb0 22 // scales value potmeter from 0-1 to -1 - 1.
brass_phoenix 0:7c204101adb0 23 return pot_value * 2 - 1;
brass_phoenix 0:7c204101adb0 24 };
brass_phoenix 0:7c204101adb0 25
brass_phoenix 0:7c204101adb0 26
brass_phoenix 0:7c204101adb0 27 int main()
brass_phoenix 0:7c204101adb0 28 {
brass_phoenix 1:28377623e8c9 29 pc.baud(115200);
brass_phoenix 0:7c204101adb0 30 pc.printf("Starting.");
brass_phoenix 6:cc3e47c7aac2 31 motor1.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 6:cc3e47c7aac2 32 motor2.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 5:fd4eaf3cb09d 33 // Start the motor controller at the desired frequency.
brass_phoenix 6:cc3e47c7aac2 34 motor1.start(pid_period);
brass_phoenix 6:cc3e47c7aac2 35 motor2.start(pid_period);
brass_phoenix 5:fd4eaf3cb09d 36
brass_phoenix 0:7c204101adb0 37 while(true){
brass_phoenix 5:fd4eaf3cb09d 38 // reads out value potmeter 1 between 0-1
brass_phoenix 6:cc3e47c7aac2 39 double pot = potmeter1.read();
brass_phoenix 5:fd4eaf3cb09d 40 double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 5:fd4eaf3cb09d 41 // Update the motor controller with the new angle.
brass_phoenix 6:cc3e47c7aac2 42 motor1.set_target_angle(desired_angle);
brass_phoenix 6:cc3e47c7aac2 43
brass_phoenix 6:cc3e47c7aac2 44 // reads out value potmeter 1 between 0-1
brass_phoenix 6:cc3e47c7aac2 45 pot = potmeter2.read();
brass_phoenix 6:cc3e47c7aac2 46 desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 6:cc3e47c7aac2 47 // Update the motor controller with the new angle.
brass_phoenix 6:cc3e47c7aac2 48 motor2.set_target_angle(desired_angle);
brass_phoenix 6:cc3e47c7aac2 49
brass_phoenix 5:fd4eaf3cb09d 50 wait(0.1);
brass_phoenix 0:7c204101adb0 51 } //Lege while loop zodat functie niet afloopt
brass_phoenix 0:7c204101adb0 52 }