Diff: src/motors.cpp

Revision:

0:88c60458332e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/motors.cpp	Thu Nov 08 06:38:19 2018 +0000
@@ -0,0 +1,73 @@
+#include "motors.h"
+#include "mbed.h"
+#include "pins.h"
+
+PwmOut m_RF(MOTOR_RF);
+PwmOut m_RB(MOTOR_RB);
+PwmOut m_LF(MOTOR_LF);
+PwmOut m_LB(MOTOR_LB);
+
+/***
+ * Assignment 2
+ * 
+ * Add logic to set the PWM based on postiive/negative number.
+ ***/
+
+void Motors::setMotorPwm(int motor, float pwm) {
+    if (abs(pwm) > MAX_SPEED) {
+        if (pwm > 0) pwm = MAX_SPEED;
+        else if (pwm < 0) pwm = -MAX_SPEED;
+    }
+    /*
+    else if (ABS_PWM < MIN_SPEED) {
+        if (pwm > 0) pwm = MIN_SPEED;
+        else if (pwm < 0) pwm = -MIN_SPEED;
+    }
+    */
+    
+    // TODO
+    // Use the "PwmOut" objects defined above
+    // Hint: Stop your backwards/forward motor before going forward/backwards.
+    if (pwm >= 0) {
+        if (motor == RIGHT_MOTOR) {
+            m_RF.period_us(PERIOD_US);
+            m_RB.pulsewidth_us(0);
+            m_RF.pulsewidth_us((int)PERIOD_US*pwm);
+        } else {
+            m_LF.period_us(PERIOD_US);
+            m_LB.pulsewidth_us(0);
+            m_LF.pulsewidth_us((int)PERIOD_US*pwm);
+        }
+    } else {
+        if (motor == RIGHT_MOTOR) {
+            m_RB.period_us(PERIOD_US);
+            m_RF.pulsewidth_us(0);
+            m_RB.pulsewidth_us((int)PERIOD_US*abs(pwm));
+        } else {
+            m_LB.period_us(PERIOD_US);
+            m_LF.pulsewidth_us(0);
+            m_LB.pulsewidth_us((int)PERIOD_US*abs(pwm));
+        }
+    }
+}
+
+Motors::Motors() {
+    // DO NOT initialize PWM!!! It breaks the mouse.
+}
+
+void Motors::stop() {
+    setRightPwm(0);
+    setLeftPwm(0);   
+}
+
+void Motors::setRightPwm(float pwm) {
+    m_rpwm = pwm;
+    setMotorPwm(RIGHT_MOTOR, pwm);
+}
+
+void Motors::setLeftPwm(float pwm) {
+    m_lpwm = pwm;
+    setMotorPwm(LEFT_MOTOR, pwm);
+}
+
+