Diff: motor_control.h

Revision:

7:fd80a0d11658

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_control.h	Mon Sep 30 13:16:04 2019 +0000
@@ -0,0 +1,83 @@
+void StopMotors();
+
+DigitalOut motorLeft1(MOTOR_LEFT_1);
+DigitalOut motorLeft2(MOTOR_LEFT_2);
+DigitalOut motorRight1(MOTOR_RIGHT_1);
+DigitalOut motorRight2(MOTOR_RIGHT_2);
+PwmOut motorLeftPowerPin(MOTOR_LEFT_POWER_PIN);
+PwmOut motorRightPowerPin(MOTOR_RIGHT_POWER_PIN);
+
+void InitMotors()
+{
+    motorLeftPowerPin.period_us(100);
+    motorRightPowerPin.period_us(100);
+    StopMotors();
+}
+
+void StopMotors()
+{
+    motorLeft1 = 0;
+    motorLeft2 = 0;
+    motorRight1 = 0;
+    motorRight2 = 0;
+    motorLeftPowerPin.write(0);
+    motorRightPowerPin.write(0);
+}
+
+void ForwardLeftMotor(float power)
+{
+    motorLeft1 = 0;
+    motorLeft2 = 1;
+    motorLeftPowerPin.write((int)power/1000);
+}
+
+void BackwardLeftMotor(float power)
+{
+    motorLeft1 = 1;
+    motorLeft2 = 0;
+    motorLeftPowerPin.write((int)power/1000);
+}
+
+void ForwardRightMotor(float power)
+{
+    motorRight1 = 0;
+    motorRight2 = 1;
+    motorRightPowerPin.write(power/1000);
+}
+
+void BackwardRightMotor(float power)
+{
+    motorRight1 = 1;
+    motorRight2 = 0;
+    motorRightPowerPin.write(power/1000);
+}
+
+void BreakMotors()
+{
+    motorLeft1 = 1;
+    motorLeft2 = 1;
+    motorRight1 = 1;
+    motorRight2 = 1;
+    motorLeftPowerPin.write(0);
+    motorRightPowerPin.write(0);
+}
+
+void RunBothMotors(float leftPower, float rightPower)
+{
+    if (leftPower > 0)
+    {
+        ForwardLeftMotor(leftPower);
+    }
+    else
+    {
+        BackwardLeftMotor(abs(leftPower));
+    }
+    if (rightPower > 0)
+    {
+        ForwardRightMotor(rightPower);
+    }
+    else
+    {
+        BackwardRightMotor(abs(rightPower));
+    }
+}
\ No newline at end of file