Fix PIDcontroller.h dependency issue

README.md

@@ -14,7 +14,7 @@ This library includes code for accessing the LSM6DS33, forked from the separate
 Add the following lines to your platformio.ini file:
 
 ~~~{.cpp}
-lib_deps = 
+lib_deps =
      Wire
      wpi-32u4-library
 ~~~
@@ -46,7 +46,7 @@ This library also includes copies of several other Arduino libraries inside it w
 
 You can use these libraries in your sketch automatically without any extra installation steps and without needing to add any extra `#include` lines to your sketch.
 
-You should avoid adding extra `#include` lines such as `#include <Pushbutton.h>` because then the Arduino IDE might try to use the standalone Pushbutton library (if you previously installed it), and it would conflict with the copy of the Pushbutton code included in this library. 
+You should avoid adding extra `#include` lines such as `#include <Pushbutton.h>` because then the Arduino IDE might try to use the standalone Pushbutton library (if you previously installed it), and it would conflict with the copy of the Pushbutton code included in this library.
 
 ## Documentation
 

library.properties

@@ -1,5 +1,5 @@
 name=wpi-32u4-library
-version=3.5.2
+version=3.5.3
 author=Pololu,WPIRoboticsEngineering
 maintainer=Greg Lewin
 sentence=Forked Romi 32U4 Arduino library

src/Chassis.cpp

@@ -3,14 +3,14 @@
 /**
  * Call init() in your setup() routine. It sets up some internal timers so that the speed controllers
  * for the wheels will work properly.
- * 
- * Here's how it works: Motor::init() starts a hardware timer on a 16 ms loop. Every time the timer 
- * "rolls over," an interrupt service routine (ISR) is called that updates the motor speeds and 
+ *
+ * Here's how it works: Motor::init() starts a hardware timer on a 16 ms loop. Every time the timer
+ * "rolls over," an interrupt service routine (ISR) is called that updates the motor speeds and
  * sets a flag to notify Chassis that it is time to calculate the control inputs.
- * 
+ *
  * When set up this way, pins 6, 12, and 13 cannot be used with analogWrite()
  * */
-void Chassis::init(void) 
+void Chassis::init(void)
 {
     Romi32U4Motor::init();
 
@@ -27,7 +27,7 @@ void Chassis::init(void)
     OCR4C = 249;   //TOP goes in OCR4C
 
     TIMSK4 = 0x04; //enable overflow interrupt
-    
+
     // re-enable interrupts
     interrupts();
 }
@@ -40,7 +40,7 @@ void Chassis::setMotorPIDcoeffs(float kp, float ki)
 
 
 /**
- * Stops the motors. It calls setMotorEfforts() so that the wheels won't lock. Use setSpeeds() if you want 
+ * Stops the motors. It calls setMotorEfforts() so that the wheels won't lock. Use setSpeeds() if you want
  * the wheels to 'lock' in place.
  * */
 void Chassis::idle(void)
@@ -66,11 +66,20 @@ void Chassis::setWheelSpeeds(float leftSpeed, float rightSpeed)
     rightMotor.setTargetSpeed(rightTicksPerInterval);
 }
 
+float Chassis::getLeftWheelSpeed(void)
+{
+    return leftMotor.speed * cmPerEncoderTick * 1000.0 / ctrlIntervalMS;
+}
+
+float Chassis::getRightWheelSpeed(void)
+{
+    return rightMotor.speed * cmPerEncoderTick * 1000.0 / ctrlIntervalMS;
+}
 
 void Chassis::setTwist(float forwardSpeed, float turningSpeed)
 {
     int16_t ticksPerIntervalFwd = (forwardSpeed * (ctrlIntervalMS / 1000.0)) / cmPerEncoderTick;
-    int16_t ticksPerIntervalTurn = (robotRadius * 3.14 / 180.0) * 
+    int16_t ticksPerIntervalTurn = (robotRadius * 3.14 / 180.0) *
                         (turningSpeed * (ctrlIntervalMS / 1000.0)) / cmPerEncoderTick;
 
     leftMotor.setTargetSpeed(ticksPerIntervalFwd - ticksPerIntervalTurn);
@@ -90,7 +99,7 @@ void Chassis::driveFor(float forwardDistance, float forwardSpeed, bool block)
     leftMotor.moveFor(delta);
     rightMotor.moveFor(delta);
 
-    if(block) 
+    if(block)
     {
         while(!checkMotionComplete()) {delay(1);}
     }
@@ -109,7 +118,7 @@ void Chassis::turnFor(float turnAngle, float turningSpeed, bool block)
     leftMotor.moveFor(-delta);
     rightMotor.moveFor(delta);
 
-    if(block) 
+    if(block)
     {
         while(!checkMotionComplete()) {delay(1);}
     }
@@ -122,9 +131,9 @@ bool Chassis::checkMotionComplete(void)
 }
 
 /**
- * ISR for timing. On overflow of Timer4, the ISR takes a 'snapshot' of the encoder counts 
+ * ISR for timing. On overflow of Timer4, the ISR takes a 'snapshot' of the encoder counts
  * and then raises a flag to let the main program know it is time to execute the PID calculations.
- * 
+ *
  * Do not edit this function -- adding lengthy function calls will cause headaches.
  * */
 ISR(TIMER4_OVF_vect)
@@ -156,3 +165,9 @@ void Chassis::printEncoderCounts(void)
     Serial.print(rightMotor.getCount());
     Serial.print('\n');
 }
+
+void Chassis::setEStop(bool eStop) {
+    // Set the EStops of the motors
+    leftMotor.setEStop(eStop);
+    rightMotor.setEStop(eStop);
+}

src/Chassis.h

@@ -5,15 +5,17 @@
 
 /** \class Chassis
  * The Chassis class manages the motors and encoders.
- * 
- * Chassis sets up a hardware-based timer on a 16ms interval. At each interrupt, it
- * reads the current encoder counts and, if controlling for speed, calculates the 
- * effort using a PID controller for each motor, which can be adjusted by the user.
- * 
- * The encoders are attached automatically and the encoders will count regardless of 
- * the state of the robot.
- * 
- * Several methods are provided for low level control to commands for driving or turning. 
+ *
+ * Chassis sets up a hardware-based timer on a 16ms interval. At each interrupt,
+ * it reads the current encoder counts and, if controlling for speed, calculates
+ * the effort using a PID controller for each motor, which can be adjusted by
+ * the user.
+ *
+ * The encoders are attached automatically and the encoders will count
+ * regardless of the state of the robot.
+ *
+ * Several methods are provided for low level control to commands for driving or
+ * turning.
  * */
 class Chassis
 {
@@ -28,19 +30,20 @@ class Chassis
 
 public:
     /** \brief Chassis constructor.
-     * 
+     *
      * @param wheelDiam Wheel diameter in cm.
      * @param ticksPerRevolution Enccoder ticks per _wheel_ revolution.
      * @param wheelTrack Distance between wheels in cm.
      * */
-    Chassis(float wheelDiam = 7, float ticksPerRevolution = 1440, float wheelTrack = 14.7) 
-        : cmPerEncoderTick(wheelDiam * M_PI / ticksPerRevolution), robotRadius(wheelTrack / 2.0)
-        {}
-
+    Chassis(float wheelDiam = 7, float ticksPerRevolution = 1440,
+            float wheelTrack = 14.7)
+        : cmPerEncoderTick(wheelDiam * M_PI / ticksPerRevolution),
+          robotRadius(wheelTrack / 2.0) {}
+
     /** \brief Initializes the chassis. Must be called in setup().
      * */
     void init(void);
-    
+
     /** \brief Sets PID coefficients for the motors. Not independent.
      * */
     void setMotorPIDcoeffs(float kp, float ki);
@@ -50,46 +53,60 @@ class Chassis
     void idle(void);
 
     /** \brief Sets motor efforts. Max speed is 420.
-     * 
+     *
      * @param leftEffort Effort for left motor
      * @param rightEffort Effort for right motor
      * */
     void setMotorEfforts(int leftEffort, int rightEffort);
 
     /** \brief Sets target wheel speeds in cm/sec.
-     * 
+     *
      * @param leftSpeed Target speed for left wheel in cm/sec
      * @param rightSpeed Target speed for right wheel in cm/sec
      * */
     void setWheelSpeeds(float leftSpeed, float rightSpeed);
 
+    /**
+     * @brief Gets the current left wheel speed in cm/sec.
+     *
+     * @return float Current speed of the left wheel in cm/sec
+     */
+    float getLeftWheelSpeed(void);
+
+    /**
+     * @brief Gets the current right wheel speed in cm/sec.
+     *
+     * @return float Current speed of the right wheel in cm/sec
+     */
+    float getRightWheelSpeed(void);
+
     /** \brief Sets target motion for the chassis.
-     * 
+     *
      * @param forwardSpeed Target forward speed in cm/sec
      * @param rightSpeed Target spin rate in deg/sec
      * */
     void setTwist(float forwardSpeed, float turningSpeed);
 
     /** \brief Commands the robot to drive at a distance and speed.
-     * 
+     *
      * The chassis will stop when the distance is reached.
-     * 
+     *
      * @param forwardDistance Target distance in cm
      * @param forwardSpeed Target speed rate in cm/sec
      * @param block If true, the function blocks until the motion is complete
      * */
     void driveFor(float forwardDistance, float forwardSpeed, bool block = false);
-    
+
     /** \brief Commands the chassis to turn a set angle.
-     * 
+     *
      * @param turnAngle Target angle to turn in degrees
      * @param turningSpeed Target spin rate in deg/sec
      * @param block If true, the function blocks until the motion is complete
      * */
     void turnFor(float turnAngle, float turningSpeed, bool block = false);
 
     /** \brief Checks if the motion commanded by driveFor() or turnFor() is done.
-     * 
+     *
      * \return Returns true if the motion is complete.
      * */
     bool checkMotionComplete(void);
@@ -100,18 +117,28 @@ class Chassis
     inline void updateEncoderDeltas();
 
     /** \brief Returns the left encoder count.
-     * 
+     *
      * @param reset Resets the encoder count if true.
      * */
-    int16_t getLeftEncoderCount(bool reset = false) 
-        {return reset ? leftMotor.getAndResetCount() : leftMotor.getCount();}
+    int16_t getLeftEncoderCount(bool reset = false)
+    {
+        return reset ? leftMotor.getAndResetCount() : leftMotor.getCount();
+    }
 
     /** \brief Returns the right encoder count.
-     * 
+     *
      * @param reset Resets the encoder count if true.
      * */
-    int16_t getRightEncoderCount(bool reset = false) 
-        {return reset ? rightMotor.getAndResetCount() : rightMotor.getCount();}
+    int16_t getRightEncoderCount(bool reset = false)
+    {
+        return reset ? rightMotor.getAndResetCount() : rightMotor.getCount();
+    }
+
+    /** \brief Sets the eStop state of the robot
+     *
+     * @param eStop The desired eStop state
+     */
+    void setEStop(bool eStop);
 };
 
 extern Chassis chassis;

src/IRdecoder.cpp

@@ -36,7 +36,7 @@ void IRDecoder::handleIRsensor(void)
   //if(!FastGPIO::Pin<14>::isInputHigh()) // could use FastGPIO for speed
   if(!digitalRead(pin)) // FALLING edge
   {
-    fallingEdge = currUS; 
+    fallingEdge = currUS;
   }
 
   else // RISING edge
@@ -49,14 +49,14 @@ void IRDecoder::handleIRsensor(void)
     lastRisingEdge = risingEdge;
 
     // bits[index] = delta; //used for debugging; obsolete
-    
+
     if(delta > 8500 && delta < 9500) // received a start pulse
     {
       index = 0;
       state = IR_PREAMBLE;
       return;
     }
-    
+
     //a pulse is supposed to be 562.5 us, but I found that it averaged 620us or so
     //with the sensor that we're using, which is NOT optimized for IR remotes --
     //it's actually optimized for sensitivity. So I set the maximum accepted pulse
@@ -81,7 +81,7 @@ void IRDecoder::handleIRsensor(void)
       else if(codeLength < 3300 && codeLength > 2700) //repeat code
       {
         state = IR_REPEAT;
-        if(((currCode ^ (currCode >> 8)) & 0x00ff0000) != 0x00ff0000) {state = IR_ERROR;} 
+        if(((currCode ^ (currCode >> 8)) & 0x00ff0000) != 0x00ff0000) {state = IR_ERROR;}
         lastReceiveTime = millis(); //not really used
       }
     }
@@ -92,13 +92,13 @@ void IRDecoder::handleIRsensor(void)
       {
         index++;
       }
-      
+
       else if(codeLength < 2500 && codeLength > 2000) //long = 1
       {
         currCode += ((uint32_t)1 << index);
         index++;
       }
-      
+
       else //error
       {
         state = IR_ERROR;
@@ -110,7 +110,7 @@ void IRDecoder::handleIRsensor(void)
         if(((currCode ^ (currCode >> 8)) & 0x00ff0000) != 0x00ff0000) state = IR_ERROR;
 
         else //we're good to go
-        {        
+        {
           state = IR_COMPLETE;
           lastReceiveTime = millis();
         }

src/IRdecoder.h

@@ -1,20 +1,20 @@
-#pragma once 
+#pragma once
 
 #include <Arduino.h>
 
 /** \class IRDecoder
- * A class to interpret IR remotes with NEC encoding. 
- * 
+ * A class to interpret IR remotes with NEC encoding.
+ *
  * NEC encoding sends four bytes: [device ID, ~divice ID, key code, ~key code]
- * 
+ *
  * Sending the inverse allow for easy error checking (and reduces saturation in the receiver).
- * 
+ *
  * Codes are send in little endian; this library reverses upon reception, so the first bit received
  * is in the LSB of currCode. That means that the key code is found in bits [23..16] of currCode
- * 
+ *
  * https://techdocs.altium.com/display/FPGA/NEC+Infrared+Transmission+Protocol
- * 
- * This does not interpret the codes into which key was pressed. That needs to be 
+ *
+ * This does not interpret the codes into which key was pressed. That needs to be
  * mapped on a remote by remote basis.
  */
 class IRDecoder
@@ -30,7 +30,7 @@ class IRDecoder
     IR_ACTIVE,   //have some bits, but not yet complete
     IR_COMPLETE, //a valid code has been received
     IR_ERROR     //an error occurred; won't return a valid code
-  }; 
+  };
 
   IR_STATE state = IR_READY; //a simple state machine for managing reception