PID controll for the robot motors.
Dependencies: BioroboticsMotorControl MODSERIAL mbed
main.cpp@5:fd4eaf3cb09d, 2018-10-19 (annotated)
- 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?
User | Revision | Line number | New 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 | } |