Code for the Poolinator
Dependencies: mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- sjoerd1999
- Date:
- 2019-10-21
- Revision:
- 6:1e9c8577f7c9
- Parent:
- 5:33133ebe37fd
- Child:
- 7:bfcb74384f46
File content as of revision 6:1e9c8577f7c9:
/* The Poolinator - A pool playing robot for people with DMD GROUP 10 Sjoerd de Jong - s1949950 Joost Loohuis - s1969633 Viktor Edlund - s2430878 Giuseppina Pinky Diatmiko - s1898841 Daan v.d Veen - s2003171 */ // THE PINOUT IS SHOWN IN THE 'PINOUT' DOC ON THE DRIVE!! // #include "mbed.h" #include "HIDScope.h" #include "QEI.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "FastPWM.h" MODSERIAL pc(USBTX, USBRX); AnalogIn pot(A1); // DC MOTORS CODE // QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32); // DELETED encoder3 for now because I need D9 for Servo PwmOut motor1_pwm(D6), motor2_pwm(D5), motor3_pwm(D3); DigitalOut motor1_dir(D7), motor2_dir(D4), motor3_dir(D2); int frequency_pwm = 10000; void setMotor(int motor, float motor_spd) // { int motor_dir = (motor_spd >= 0) ? 0 : 1; motor_spd = fabs(motor_spd); if(motor_spd > 1) motor_spd = 1; if(motor_spd < 0.1) motor_spd = 0; // Don't turn if too small else motor_spd = (motor_spd * 0.5) + 0.5; // else, map between 0.5 and 1, motor doesn't turn when smaller if(motor == 1) { motor1_dir.write(motor_dir); motor1_pwm.write(motor_spd); } else if(motor == 2) { motor2_dir.write(motor_dir); motor2_pwm.write(motor_spd); } else if(motor == 3) { motor3_dir.write(motor_dir); motor3_pwm.write(motor_spd); } } // PID CODE // 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) double Ts = 0.01; // Sample time in seconds double PID_gain = 0.00075; // Because we use counts instead of radians, 1/8400*2PI=0.00075 static double error_integral = 0; static double error_prev = 0; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); double pid(double error) { // Proportional part: double u_k = Kp * error; // Integral part error_integral = error_integral + error * Ts; 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 double u_i = Ki * error_integral; // Derivative part double error_derivative = (error - error_prev) / Ts; double filtered_error_derivative = LowPassFilter.step(error_derivative); double u_d = Kd * filtered_error_derivative; error_prev = error; // Sum all parts and return it return u_k + u_i + u_d; } // STEPPER MOTOR CODE // DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8); int stepper_steps = 0; float stepper_angle = 0; void stepper_move(int direction_) // Requires ~1.5ms wait time between each step { STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7); STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5); STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3); STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1); stepper_steps += (direction_ == 0) ? - 1 : 1; stepper_steps = (stepper_steps + 8) % 8; stepper_angle += ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00); } // SERVO CODE // PwmOut servo(D9); void moveServo(float i) { float duty = (i + 20) / 1000.00; servo.write(duty); } // SOLENOID CODE // DigitalOut solenoidA(PTC0), solenoidB(PTC9); void setSolenoid(int dir) { solenoidA = (dir == 0) ? 0 : 1; solenoidB = (dir == 0) ? 1 : 0; } int main() { pc.baud(115200); pc.printf("\r\nStarting...\r\n\r\n"); motor1_pwm.period(1.0 / frequency_pwm); // T = 1 / f motor2_pwm.period(1.0 / frequency_pwm); motor3_pwm.period(1.0 / frequency_pwm); servo.period(0.020); while (true) { /* SOME EXAPLE CODE * MOTOR setMotor(..., ...) // which motor (1,2,3), and speed (-inf, +inf) * PID double currentPos1 = encoder1.getPulses(); double targetPos1 = 10000; double error1 = targetPos1 - currentPos1; float PID_speed1 = pid(error) * PID_gain; setMotor(1, PID_speed1); * STEPPER stepper_move(1); // direction (0 or 1) wait_ms(1.5); // requires some time between steps * SERVO moveServo(...) // value between 0.0 and 100.0 (= 0 and 180 degrees) * SOLENOID setSolenoid(0); // shooting wait_ms(1000); setSolenoid(1); */ // DEMO //for(int i = 0; i < 100; i++) { // moveServo(i); // wait_ms(50); // } // for(int i = 100; i > 0; i--) { // moveServo(i); // wait_ms(50); // } // for(int i = 0; i < 2000; i++) { // stepper_move(1); // direction (0 or 1) // wait_ms(1.5); // requires some time between steps // } // for(int i = 0; i < 2000; i++) { // stepper_move(0); // direction (0 or 1) // wait_ms(1.5); // requires some time between steps // } // setSolenoid(1); // wait_ms(1000); // setSolenoid(0); } }