Revision 2:f8e0a7b5c90a, committed 2019-10-14

Comitter:

sjoerd1999

Date:

Mon Oct 14 09:47:49 2019 +0000

Parent:

1:b862262a9d14

Child:

3:d319bc2b19f1

Commit message:

V1

Changed in this revision

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Mon Oct 14 09:47:49 2019 +0000
@@ -1,23 +1,67 @@
 #include "mbed.h"
 //#include "HIDScope.h"
-//#include "QEI.h"
+#include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
-
-DigitalOut led(LED_RED);
+#include "BiQuad.h"
+#include "FastPWM.h"
 
 MODSERIAL pc(USBTX, USBRX);
 
+// DC MOTORS CODE //
+QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32), encoder3(D8,D9,NC,32);
+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; // 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);
+    }
+}
+
+
+
+// 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_)
+{
+    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;
+    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) ? - 360.00 / 4096.00 : 360.00 / 4096.00;
+}
+
+
+
 int main()
 {
     pc.baud(115200);
     pc.printf("\r\nStarting...\r\n\r\n");
-    
+
     while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
+        stepper_move(1);
+        wait_ms(1.5);
     }
 }