Biorobotics 2019 - Group 10 - The Poolinator / Mbed 2 deprecated Poolinator

Code for the Poolinator

Dependencies:   mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM

main.cpp@3:d319bc2b19f1, 2019-10-14 (annotated)

Committer:
sjoerd1999
Date:
Mon Oct 14 10:21:10 2019 +0000
Revision:
3:d319bc2b19f1
Parent:
2:f8e0a7b5c90a
Child:
4:8113394bed1b
Added Servo/Solenoid control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
sjoerd1999 3:d319bc2b19f1 2 #include "HIDScope.h"
sjoerd1999 2:f8e0a7b5c90a 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
sjoerd1999 2:f8e0a7b5c90a 5 #include "BiQuad.h"
sjoerd1999 2:f8e0a7b5c90a 6 #include "FastPWM.h"
sjoerd1999 3:d319bc2b19f1 7 #include "Servo.h"
sjoerd1999 3:d319bc2b19f1 8
sjoerd1999 3:d319bc2b19f1 9 // PLEASE USE THE PINOUT AS SHOWN IN THE PINOUT DOC ON THE DRIVE
RobertoO 0:67c50348f842 10
RobertoO 1:b862262a9d14 11 MODSERIAL pc(USBTX, USBRX);
RobertoO 0:67c50348f842 12
sjoerd1999 3:d319bc2b19f1 13 AnalogIn pot(A1);
sjoerd1999 3:d319bc2b19f1 14
sjoerd1999 2:f8e0a7b5c90a 15 // DC MOTORS CODE //
sjoerd1999 2:f8e0a7b5c90a 16 QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32), encoder3(D8,D9,NC,32);
sjoerd1999 2:f8e0a7b5c90a 17 PwmOut motor1_pwm(D6), motor2_pwm(D5), motor3_pwm(D3);
sjoerd1999 2:f8e0a7b5c90a 18 DigitalOut motor1_dir(D7), motor2_dir(D4), motor3_dir(D2);
sjoerd1999 2:f8e0a7b5c90a 19 int frequency_pwm = 10000;
sjoerd1999 2:f8e0a7b5c90a 20
sjoerd1999 3:d319bc2b19f1 21 void setMotor(int motor, float motor_spd) //
sjoerd1999 2:f8e0a7b5c90a 22 {
sjoerd1999 2:f8e0a7b5c90a 23 int motor_dir = (motor_spd >= 0) ? 0 : 1;
sjoerd1999 2:f8e0a7b5c90a 24
sjoerd1999 2:f8e0a7b5c90a 25 motor_spd = fabs(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 26 if(motor_spd > 1) motor_spd = 1;
sjoerd1999 2:f8e0a7b5c90a 27 if(motor_spd < 0.1) motor_spd = 0; // Don't turn if too small
sjoerd1999 3:d319bc2b19f1 28 else motor_spd = (motor_spd * 0.5) + 0.5; // else, map between 0.5 and 1, motor doesn't turn when smaller
sjoerd1999 2:f8e0a7b5c90a 29
sjoerd1999 2:f8e0a7b5c90a 30 if(motor == 1) {
sjoerd1999 2:f8e0a7b5c90a 31 motor1_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 32 motor1_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 33 } else if(motor == 2) {
sjoerd1999 2:f8e0a7b5c90a 34 motor2_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 35 motor2_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 36 } else if(motor == 3) {
sjoerd1999 2:f8e0a7b5c90a 37 motor3_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 38 motor3_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 39 }
sjoerd1999 2:f8e0a7b5c90a 40 }
sjoerd1999 2:f8e0a7b5c90a 41
sjoerd1999 2:f8e0a7b5c90a 42
sjoerd1999 2:f8e0a7b5c90a 43
sjoerd1999 3:d319bc2b19f1 44 // PID CODE //
sjoerd1999 3:d319bc2b19f1 45 double Kp = 5.5, Ki = 1.02, Kd = 0.0; // No Kd, makes the motor wiggle back and forth, dunno why (default: 17.5, 1.02, 23.2)
sjoerd1999 3:d319bc2b19f1 46 double Ts = 0.01; // Sample time in seconds
sjoerd1999 3:d319bc2b19f1 47 double PID_gain = 0.00075; // Because we use counts instead of radians, 1/8400*2PI=0.00075
sjoerd1999 3:d319bc2b19f1 48
sjoerd1999 3:d319bc2b19f1 49 static double error_integral = 0;
sjoerd1999 3:d319bc2b19f1 50 static double error_prev = 0;
sjoerd1999 3:d319bc2b19f1 51 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
sjoerd1999 3:d319bc2b19f1 52
sjoerd1999 3:d319bc2b19f1 53 double pid(double error)
sjoerd1999 3:d319bc2b19f1 54 {
sjoerd1999 3:d319bc2b19f1 55 // Proportional part:
sjoerd1999 3:d319bc2b19f1 56 double u_k = Kp * error;
sjoerd1999 3:d319bc2b19f1 57
sjoerd1999 3:d319bc2b19f1 58 // Integral part
sjoerd1999 3:d319bc2b19f1 59 error_integral = error_integral + error * Ts;
sjoerd1999 3:d319bc2b19f1 60 if(error > 1000) error_integral = 0; // Only apply the I-action if the error is small, otherwise speed will keep building up and cause big overshoots
sjoerd1999 3:d319bc2b19f1 61 double u_i = Ki * error_integral;
sjoerd1999 3:d319bc2b19f1 62
sjoerd1999 3:d319bc2b19f1 63 // Derivative part
sjoerd1999 3:d319bc2b19f1 64 double error_derivative = (error - error_prev) / Ts;
sjoerd1999 3:d319bc2b19f1 65 double filtered_error_derivative = LowPassFilter.step(error_derivative);
sjoerd1999 3:d319bc2b19f1 66 double u_d = Kd * filtered_error_derivative;
sjoerd1999 3:d319bc2b19f1 67 error_prev = error;
sjoerd1999 3:d319bc2b19f1 68
sjoerd1999 3:d319bc2b19f1 69 // Sum all parts and return it
sjoerd1999 3:d319bc2b19f1 70 return u_k + u_i + u_d;
sjoerd1999 3:d319bc2b19f1 71 }
sjoerd1999 3:d319bc2b19f1 72
sjoerd1999 3:d319bc2b19f1 73
sjoerd1999 3:d319bc2b19f1 74
sjoerd1999 2:f8e0a7b5c90a 75 // STEPPER MOTOR CODE //
sjoerd1999 2:f8e0a7b5c90a 76 DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);
sjoerd1999 2:f8e0a7b5c90a 77
sjoerd1999 2:f8e0a7b5c90a 78 int stepper_steps = 0;
sjoerd1999 2:f8e0a7b5c90a 79 float stepper_angle = 0;
sjoerd1999 2:f8e0a7b5c90a 80
sjoerd1999 3:d319bc2b19f1 81 void stepper_move(int direction_) // Requires ~1.5ms wait time between each step
sjoerd1999 2:f8e0a7b5c90a 82 {
sjoerd1999 3:d319bc2b19f1 83 STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
sjoerd1999 2:f8e0a7b5c90a 84 STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
sjoerd1999 2:f8e0a7b5c90a 85 STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
sjoerd1999 2:f8e0a7b5c90a 86 STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
sjoerd1999 2:f8e0a7b5c90a 87 stepper_steps += (direction_ == 0) ? - 1 : 1;
sjoerd1999 2:f8e0a7b5c90a 88 stepper_steps = (stepper_steps + 8) % 8;
sjoerd1999 3:d319bc2b19f1 89 stepper_angle += ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00);
sjoerd1999 2:f8e0a7b5c90a 90 }
sjoerd1999 2:f8e0a7b5c90a 91
sjoerd1999 2:f8e0a7b5c90a 92
sjoerd1999 2:f8e0a7b5c90a 93
sjoerd1999 3:d319bc2b19f1 94 // SERVO CODE //
sjoerd1999 3:d319bc2b19f1 95 Servo myservo(PTC9);
sjoerd1999 3:d319bc2b19f1 96
sjoerd1999 3:d319bc2b19f1 97
sjoerd1999 3:d319bc2b19f1 98
sjoerd1999 3:d319bc2b19f1 99 // SOLENOID CODE //
sjoerd1999 3:d319bc2b19f1 100 DigitalOut solenoidA(PTC0), solenoidB(PTC7);
sjoerd1999 3:d319bc2b19f1 101
sjoerd1999 3:d319bc2b19f1 102 void setSolenoid(int dir)
sjoerd1999 3:d319bc2b19f1 103 {
sjoerd1999 3:d319bc2b19f1 104 solenoidA = (dir == 0) ? 0 : 1;
sjoerd1999 3:d319bc2b19f1 105 solenoidB = (dir == 0) ? 1 : 0;
sjoerd1999 3:d319bc2b19f1 106 }
sjoerd1999 3:d319bc2b19f1 107
sjoerd1999 3:d319bc2b19f1 108
sjoerd1999 3:d319bc2b19f1 109
sjoerd1999 3:d319bc2b19f1 110
RobertoO 0:67c50348f842 111 int main()
RobertoO 0:67c50348f842 112 {
RobertoO 0:67c50348f842 113 pc.baud(115200);
RobertoO 1:b862262a9d14 114 pc.printf("\r\nStarting...\r\n\r\n");
sjoerd1999 2:f8e0a7b5c90a 115
RobertoO 0:67c50348f842 116 while (true) {
sjoerd1999 3:d319bc2b19f1 117 // stepper_move(1);
sjoerd1999 3:d319bc2b19f1 118 // wait_ms(1.5);
sjoerd1999 3:d319bc2b19f1 119
sjoerd1999 3:d319bc2b19f1 120 // myservo = ... (value between 0.0 and 1.0)
RobertoO 0:67c50348f842 121 }
RobertoO 0:67c50348f842 122 }