Major simplification

lib/Chassis/src/chassis.cpp

@@ -108,9 +108,6 @@ bool Chassis::ChassisLoop(Twist& velocity)
         Serial.print('\n');
 #endif
 
-        // motor updates
-        UpdateMotors();
-
         /* Update the wheel velocity so it gets back to Robot. */
         velocity = CalcOdomFromWheelMotion();
 
@@ -139,30 +136,6 @@ void Chassis::SetMotorEfforts(int16_t left, int16_t right)
     rightMotor.SetMotorEffortDirect(right);
 }
 
-void Chassis::UpdateMotors(void)
-{
-    leftMotor.ControlMotorSpeed();
-    rightMotor.ControlMotorSpeed();
-}
-
-/**
- * SetWheelSpeeds converts the linear wheel speeds (axes relative to ground) to motor speeds.
- */
-void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec)
-{
-    /** 
-     * TODO: Check the code below. You did this in Lab 1, so we give you the calcs.
-     */
-    leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.);
-    rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.);
-}
-
-void Chassis::SetTwist(const Twist& twist)
-{
-    /**
-     * TODO: Complete SetTwist() to call SetWheelSpeeds() from target u and omega
-     */
-}
 
 Twist Chassis::CalcOdomFromWheelMotion(void)
 {

lib/Chassis/src/chassis.h

@@ -42,12 +42,10 @@ public:
     void SetTwist(const Twist& twist);
     Twist CalcOdomFromWheelMotion(void);
 
-    /**
+
-     * A utility function for converting robot speed to wheel speed. Left 
+    void SetMotorEfforts(int16_t, int16_t);
-     * public so that you can test more easily.
+
-     */
+    void Stop(void) { SetMotorEfforts(0, 0);} 
-    void SetWheelSpeeds(float, float);
-    void Stop(void) { SetWheelSpeeds(0, 0); }
 
 protected:
     /**
@@ -57,5 +55,4 @@ protected:
     void InitializeMotors(void);
     
     void UpdateMotors(void);
-    void SetMotorEfforts(int16_t, int16_t);
 };

lib/Romi32U4Motors/src/Romi32U4MotorParameters.h

@@ -1,5 +0,0 @@
-#define KP_MOTOR    5.0
-#define KI_MOTOR    0.5
-#define KD_MOTOR    0.0             // don't use
-
-#define INTEGRAL_CAP_MOTOR  0.0     // if you want to add an integral cap

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -2,7 +2,6 @@
 
 #include <Arduino.h>
 #include <FastGPIO.h>
-#include "Romi32U4MotorParameters.h"
 
 // define the motor pins here
 #define PWM_L 10
@@ -24,30 +23,12 @@ protected:
     // For prettier printing
     const String name;
 
-    // Used to control the motors in different ways
-    enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
-    volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
-
-    // Used to manage PID coefficients; note that the defaults stink for speed control.
-    float Kp = KP_MOTOR;
-    float Ki = KI_MOTOR;
-    float Kd = KD_MOTOR;
-
-    // Used to keep track of the target speed, in counts / interval.
-    float targetSpeed = 0;
-
     /**
      * This is the speed of the motor, in "encoder counts / encoder interval".
      * The encoder interval is set in Robot::InitializeMotorControlTimer.
      */ 
     volatile int16_t speed = 0;
 
-    // For tracking the error integral (sum)
-    float sumError = 0;
-
-    // For tracking derivative (difference)
-    float prevError = 0;
-
     // Maximum effort
     int16_t maxEffort = 420;
 
@@ -69,7 +50,6 @@ protected:
      */
     void SetMotorEffortDirect(int16_t effort)
     {
-        ctrlMode = CTRL_DIRECT;
         SetEffort(effort);
     }
 
@@ -93,63 +73,19 @@ protected:
     }
 
     /**
-     * Sets the target speed in "encoder ticks/16 ms interval"
+     * GetEncoderTotal() returns the total encoder counts" 
-     * */
+     * 
-    void SetTargetSpeed(float target)
+     */
+    int16_t GetEncoderTotal(void)
     {
-        targetSpeed = target;
+        cli();
+        int16_t currCount = encCount;
+        sei();
 
-        if(ctrlMode != CTRL_SPEED)
+        return currCount;
-        {
-            // Reset the error integral if we are switching from another mode
-            // Otherwise, the robot may jump due to residual integral
-            sumError = 0;
-
-            // Also set prevCount to encCount so to avoid speed jumps when switching mode
-            CalcEncoderDelta();
-        }
-
-        ctrlMode = CTRL_SPEED;
     }
 
-    /**
-     * ControlMotorSpeed implements the PID controller. It should _not_ be called by user code.
-     * Instead, ControlMotorSpeed is called from Chassis::UpdateMotors, which is on a timer schedule.
-    */
-    void ControlMotorSpeed(void)
-    {
-        if(ctrlMode == CTRL_SPEED)
-        {
-            /**
-             * TODO: Implement PI controller. Only P at the moment.
-             */
 
-            // Calculate the error in speed
-            float error = targetSpeed - speed;
-
-            sumError+= error;
-
-            int16_t effort = Kp * error + Ki * sumError;
-
-            // Set the effort for the motor
-            SetEffort(effort);
-
-#ifdef __MOTOR_DEBUG__
-                Serial.print('>');
-                Serial.print(name);
-                Serial.print("_target:");
-                Serial.println(targetSpeed);
-                Serial.print('>');
-                Serial.print(name);
-                Serial.print("_speed:");
-                Serial.println(speed);
-                Serial.print('>');
-                Serial.print(name);
-                Serial.print("_error:");
-                Serial.println(error);
-#endif
-        }    
-    }
 
     static void AttachInterrupts(void);
 
@@ -193,7 +129,6 @@ protected:
 
 public:
     Romi32U4MotorBase(const String& nm) : name(nm) {}
-    void SetPIDCoeffs(float p, float i, float d) {Kp = p; Ki = i; Kd = d; sumError = 0;}
 };
 
 template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR> 

platformio.ini

@@ -17,7 +17,6 @@ monitor_speed = 115200
 
 lib_deps = 
     Wire
-    https://github.com/gcl8a/IRDecoder
     https://github.com/gcl8a/Button 
     https://github.com/gcl8a/event_timer
     https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities

src/robot-nav.cpp

@@ -59,7 +59,7 @@ void Robot::DriveToPoint(void)
 #endif
 
         /**
-         * TODO: Call chassis.SetTwist() to command the motion, based on your calculations above.
+         * TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
          */
     }
 }

src/robot-remote.cpp

@@ -1,83 +0,0 @@
-/**
- * robot-remote.cpp implements features of class Robot that are related to processing
- * remote control commands. It also manages modes. You might want to trim away some of 
- * the code that isn't needed later in the term.
- */
-#include "robot.h"
-
-#include <ir_codes.h>
-#include <IRdecoder.h>
-
-/**
- * IRDecoder decoder is declared extern in IRdecoder.h (for ISR purposes), 
- * so we _must_ name it decoder.
- */
-#define IR_PIN 17
-IRDecoder decoder(IR_PIN);
-
-void Robot::HandleKeyCode(int16_t keyCode)
-{ 
-    Serial.println(keyCode);
-
-    // Regardless of current state, if ENTER is pressed, go to idle state
-    if(keyCode == ENTER_SAVE) { EnterIdleState(); keyString = "";}
-
-
-    // If STOP/MODE is pressed, it toggles control mode (auto <-> teleop)
-    else if(keyCode == STOP_MODE) 
-    {
-        if(robotCtrlMode == CTRL_AUTO) {EnterTeleopMode(); }
-        else if(robotCtrlMode == CTRL_TELEOP) {EnterAutoMode(); }
-        EnterIdleState(); // Idle to avoid surprises
-        keyString = "";
-    }
-
-    if(robotCtrlMode == CTRL_AUTO)
-    {
-        /**
-         * TODO: Add destinations.
-         */
-        switch(keyCode)
-        {
-            case NUM_2:
-                SetDestination(Pose(60, 0, 0)); 
-                break;
-            case NUM_4:
-                break;
-            case NUM_6:
-                break;
-            case NUM_8:
-                break;
-        }
-        keyString = "";
-    }
-
-    else if(robotCtrlMode == CTRL_TELEOP)
-    {
-        switch(keyCode)
-        {
-            case UP_ARROW:
-                chassis.SetTwist(Twist(20, 0, 0));
-                break;
-            case RIGHT_ARROW:
-                break;
-            case DOWN_ARROW:
-                break;
-            case LEFT_ARROW:
-                break;
-        }
-        keyString = "";
-    }    
-}
-
-void Robot::EnterTeleopMode(void)
-{
-    Serial.println("-> TELEOP");
-    robotCtrlMode = CTRL_TELEOP;
-}
-
-void Robot::EnterAutoMode(void)
-{
-    Serial.println("-> AUTO");
-    robotCtrlMode = CTRL_AUTO;
-}

src/robot.cpp

@@ -1,16 +1,9 @@
 #include "robot.h"
-#include <IRdecoder.h>
 
 void Robot::InitializeRobot(void)
 {
     chassis.InititalizeChassis();
 
-    /**
-     * Initialize the IR decoder. Declared extern in IRdecoder.h; see robot-remote.cpp
-     * for instantiation and setting the pin.
-     */
-    decoder.init();
-
     /**
      * TODO: Set pin 13 HIGH when navigating and LOW when destination is reached.
      * Need to set as OUTPUT here.
@@ -27,17 +20,11 @@ void Robot::EnterIdleState(void)
 
 /**
  * The main loop for your robot. Process both synchronous events (motor control),
- * and asynchronous events (IR presses, distance readings, etc.).
+ * and asynchronous events (distance readings, etc.).
 */
 void Robot::RobotLoop(void) 
 {
-    /**
+     /**
-     * Handle any IR remote keypresses.
-     */
-    int16_t keyCode = decoder.getKeyCode();
-    if(keyCode != -1) HandleKeyCode(keyCode);
-
-    /**
      * Run the chassis loop, which handles low-level control.
      */
     Twist velocity;
@@ -45,6 +32,7 @@ void Robot::RobotLoop(void)
     {
         // We do FK regardless of state
         UpdatePose(velocity);
+        chassis.SetMotorEfforts(220,-220);
         
         /**
          * Here, we break with tradition and only call these functions if we're in the 

src/robot.h

@@ -5,17 +5,6 @@
 class Robot
 {
 protected:
-    /**
-     * We define some modes for you. SETUP is used for adjusting gains and so forth. Most
-     * of the activities will run in AUTO. You shouldn't need to mess with these.
-     */
-    enum ROBOT_CTRL_MODE
-    {
-        CTRL_TELEOP,
-        CTRL_AUTO,
-    };
-    ROBOT_CTRL_MODE robotCtrlMode = CTRL_AUTO;
-
     /**
      * robotState is used to track the current task of the robot. You will add new states as 
      * the term progresses.
@@ -45,16 +34,9 @@ public:
     void RobotLoop(void);
 
 protected:
-    /* For managing IR remote key presses*/
-    void HandleKeyCode(int16_t keyCode);
-
     /* State changes */    
     void EnterIdleState(void);
 
-    /* Mode changes */
-    void EnterTeleopMode(void);
-    void EnterAutoMode(void);
-
     // /* Navigation methods.*/
     void UpdatePose(const Twist& u);
     void SetDestination(const Pose& destination);