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?
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 | 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 | } |