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

Code for the Poolinator

Dependencies:   mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM

main.cpp@2:f8e0a7b5c90a, 2019-10-14 (annotated)

Committer:
sjoerd1999
Date:
Mon Oct 14 09:47:49 2019 +0000
Revision:
2:f8e0a7b5c90a
Parent:
1:b862262a9d14
Child:
3:d319bc2b19f1
V1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 0:67c50348f842 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"
RobertoO 0:67c50348f842 7
RobertoO 1:b862262a9d14 8 MODSERIAL pc(USBTX, USBRX);
RobertoO 0:67c50348f842 9
sjoerd1999 2:f8e0a7b5c90a 10 // DC MOTORS CODE //
sjoerd1999 2:f8e0a7b5c90a 11 QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32), encoder3(D8,D9,NC,32);
sjoerd1999 2:f8e0a7b5c90a 12 PwmOut motor1_pwm(D6), motor2_pwm(D5), motor3_pwm(D3);
sjoerd1999 2:f8e0a7b5c90a 13 DigitalOut motor1_dir(D7), motor2_dir(D4), motor3_dir(D2);
sjoerd1999 2:f8e0a7b5c90a 14 int frequency_pwm = 10000;
sjoerd1999 2:f8e0a7b5c90a 15
sjoerd1999 2:f8e0a7b5c90a 16 void setMotor(int motor, float motor_spd) //
sjoerd1999 2:f8e0a7b5c90a 17 {
sjoerd1999 2:f8e0a7b5c90a 18 int motor_dir = (motor_spd >= 0) ? 0 : 1;
sjoerd1999 2:f8e0a7b5c90a 19
sjoerd1999 2:f8e0a7b5c90a 20 motor_spd = fabs(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 21 if(motor_spd > 1) motor_spd = 1;
sjoerd1999 2:f8e0a7b5c90a 22 if(motor_spd < 0.1) motor_spd = 0; // Don't turn if too small
sjoerd1999 2:f8e0a7b5c90a 23 else motor_spd = (motor_spd * 0.5) + 0.5; // map between 0.5 and 1, motor doesn't turn when smaller
sjoerd1999 2:f8e0a7b5c90a 24
sjoerd1999 2:f8e0a7b5c90a 25 if(motor == 1) {
sjoerd1999 2:f8e0a7b5c90a 26 motor1_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 27 motor1_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 28 } else if(motor == 2) {
sjoerd1999 2:f8e0a7b5c90a 29 motor2_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 30 motor2_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 31 } else if(motor == 3) {
sjoerd1999 2:f8e0a7b5c90a 32 motor3_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 33 motor3_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 34 }
sjoerd1999 2:f8e0a7b5c90a 35 }
sjoerd1999 2:f8e0a7b5c90a 36
sjoerd1999 2:f8e0a7b5c90a 37
sjoerd1999 2:f8e0a7b5c90a 38
sjoerd1999 2:f8e0a7b5c90a 39 // STEPPER MOTOR CODE //
sjoerd1999 2:f8e0a7b5c90a 40 DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);
sjoerd1999 2:f8e0a7b5c90a 41
sjoerd1999 2:f8e0a7b5c90a 42 int stepper_steps = 0;
sjoerd1999 2:f8e0a7b5c90a 43 float stepper_angle = 0;
sjoerd1999 2:f8e0a7b5c90a 44
sjoerd1999 2:f8e0a7b5c90a 45 void stepper_move(int direction_)
sjoerd1999 2:f8e0a7b5c90a 46 {
sjoerd1999 2:f8e0a7b5c90a 47 STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7); // If steps is equal to either of those numbers, set the pin to HIGH, else LOW;
sjoerd1999 2:f8e0a7b5c90a 48 STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
sjoerd1999 2:f8e0a7b5c90a 49 STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
sjoerd1999 2:f8e0a7b5c90a 50 STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
sjoerd1999 2:f8e0a7b5c90a 51 stepper_steps += (direction_ == 0) ? - 1 : 1;
sjoerd1999 2:f8e0a7b5c90a 52 stepper_steps = (stepper_steps + 8) % 8;
sjoerd1999 2:f8e0a7b5c90a 53 stepper_angle += (direction_ == 0) ? - 360.00 / 4096.00 : 360.00 / 4096.00;
sjoerd1999 2:f8e0a7b5c90a 54 }
sjoerd1999 2:f8e0a7b5c90a 55
sjoerd1999 2:f8e0a7b5c90a 56
sjoerd1999 2:f8e0a7b5c90a 57
RobertoO 0:67c50348f842 58 int main()
RobertoO 0:67c50348f842 59 {
RobertoO 0:67c50348f842 60 pc.baud(115200);
RobertoO 1:b862262a9d14 61 pc.printf("\r\nStarting...\r\n\r\n");
sjoerd1999 2:f8e0a7b5c90a 62
RobertoO 0:67c50348f842 63 while (true) {
sjoerd1999 2:f8e0a7b5c90a 64 stepper_move(1);
sjoerd1999 2:f8e0a7b5c90a 65 wait_ms(1.5);
RobertoO 0:67c50348f842 66 }
RobertoO 0:67c50348f842 67 }