PID controll for the robot motors.

Dependencies:   BioroboticsMotorControl MODSERIAL mbed

Committer:
brass_phoenix
Date:
Fri Oct 19 10:51:16 2018 +0000
Revision:
5:fd4eaf3cb09d
Parent:
4:fce8c7458e97
Child:
6:cc3e47c7aac2
+ Motor control is now a separate class.

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 1:28377623e8c9 4 #include "pid.h"
brass_phoenix 5:fd4eaf3cb09d 5 #include "motor.h"
brass_phoenix 1:28377623e8c9 6
brass_phoenix 4:fce8c7458e97 7 const float pid_period = 0.001; // PID sample period in seconds.
brass_phoenix 0:7c204101adb0 8
brass_phoenix 3:689f3f2e78e8 9 const double Kp = 10.0;
brass_phoenix 4:fce8c7458e97 10 const double Ki = 0.1;
brass_phoenix 3:689f3f2e78e8 11 const double Kd = 0.5;
brass_phoenix 2:3be8cd780b3d 12
brass_phoenix 0:7c204101adb0 13 AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 0:7c204101adb0 14 AnalogIn potmeter2(A2); // Analoge input van potmeter 2 -> Motor 2
brass_phoenix 0:7c204101adb0 15 Serial pc(USBTX, USBRX);
brass_phoenix 0:7c204101adb0 16
brass_phoenix 5:fd4eaf3cb09d 17 Motor motor(D6, D7, D13, D12, &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 5:fd4eaf3cb09d 31 motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 5:fd4eaf3cb09d 32 // Start the motor controller at the desired frequency.
brass_phoenix 5:fd4eaf3cb09d 33 motor.start(pid_period);
brass_phoenix 5:fd4eaf3cb09d 34
brass_phoenix 0:7c204101adb0 35 while(true){
brass_phoenix 5:fd4eaf3cb09d 36 // reads out value potmeter 1 between 0-1
brass_phoenix 5:fd4eaf3cb09d 37 double pot = potmeter2.read();
brass_phoenix 5:fd4eaf3cb09d 38 double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
brass_phoenix 5:fd4eaf3cb09d 39 // Update the motor controller with the new angle.
brass_phoenix 5:fd4eaf3cb09d 40 motor.set_target_angle(desired_angle);
brass_phoenix 5:fd4eaf3cb09d 41 wait(0.1);
brass_phoenix 0:7c204101adb0 42 } //Lege while loop zodat functie niet afloopt
brass_phoenix 0:7c204101adb0 43 }