Romi32U4Motors.h

Go to the documentation of this file.

// Adapted from a library by Pololu Corporation.  For more information, see http://www.pololu.com/
 
#pragma once
 
#include <Arduino.h>
#include <stdint.h>
#include <PIDController.h>
 
class Romi32U4Motor
{
protected:
   // Used to control the motors in different ways
   enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED, CTRL_POS};
   volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
 
   // this is the 'speed' of the motor, in "encoder counts / 16 ms interval"
   volatile int16_t speed = 0;
 
   // used to set target speed or position (or both)
   float targetSpeed = 0;
   int16_t targetPos = 0;
 
   // Maximum effort (to protect the gear boxes). Can be changed by setting turbo mode
   int16_t maxEffort = 300;
 
   // keeps track of encoder changes
   volatile int16_t prevCount = 0;
   volatile int16_t count = 0;
   volatile int16_t lastA = 0;
   volatile int16_t lastB = 0;
 
   // We build a PID controller into the object for controlling speed
   PIDController pidCtrl;
 
   friend class Chassis;
 
public:
   Romi32U4Motor(void) : pidCtrl(5, 0.5) {} // These values work well for motor control
 
   static void init()
   {
      initMotors();
      initEncoders();
   }
 
   void setPIDCoeffients(float kp, float ki)
   {
      pidCtrl.setKp(kp);
      pidCtrl.setKi(ki);
   }
 
protected:
   // Used to set up motors and encoders. Do not call directly; they are called from init(), which 
   // is called from Chassis::init()
   static void initMotors();
   static void initEncoders();
 
  virtual void setEffort(int16_t effort) = 0;
 
   int16_t getCount(void);
   int16_t getAndResetCount(void);
 
   void setTargetSpeed(float targetSpeed);
   void moveFor(int16_t amount);
   bool checkComplete(void) {return ctrlMode == CTRL_DIRECT;}
 
   void update(void);
   void calcEncoderDelta(void);
 
public:
   void allowTurbo(bool turbo);
 
   inline void handleISR(bool newA, bool newB);
};
 
class LeftMotor : public Romi32U4Motor
{
protected:
   void setEffort(int16_t effort);
 
public: 
   void setMotorEffort(int16_t effort)
   {
      ctrlMode = CTRL_DIRECT;
      setEffort(effort);
   }
};
 
class RightMotor : public Romi32U4Motor
{
protected:
   void setEffort(int16_t effort);
 
public:
   void setMotorEffort(int16_t effort)
   {
      ctrlMode = CTRL_DIRECT;
      setEffort(effort);
   }
};