#pragma once #include #include // define the motor pins here #define PWM_L 10 #define PWM_R 9 #define DIR_L 16 #define DIR_R 15 #define LEFT_XOR 8 #define LEFT_B IO_E2 #define RIGHT_XOR 7 #define RIGHT_B 23 #define OCR_L 0x8A #define OCR_R 0x88 class Romi32U4MotorBase { protected: // Used to control the motors in different ways enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED}; volatile CTRL_MODE ctrlMode = CTRL_DIRECT; // TODO: After you tune your motors, set the gains here. float Kp = 1; float Ki = 0; float Kd = 0; float Kf = 0; // 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; // Keeps track of encoder changes volatile int16_t prevCount; volatile int16_t encCount; volatile int8_t lastA; volatile int8_t lastB; /** * */ virtual void SetEffort(int16_t effort) = 0; /** * Used to set the motor _effort_ directly, which is mostly used for testing. * * Calling this function will switch the control mode to DIRECT, if needed, meaning speed control is lost. */ void SetMotorEffortDirect(int16_t effort) { ctrlMode = CTRL_DIRECT; SetEffort(effort); } /** * calcEncoderDelta() takes a 'snapshot of the encoders and * stores the change since the last call, which has units of "encoder ticks/motor interval" * * We also use the function for zeroing the delta (for example, when switching control modes), * so interrupts are cleared when accessing encCount. */ int16_t CalcEncoderDelta(void) { cli(); int16_t currCount = encCount; sei(); int16_t speed = currCount - prevCount; prevCount = currCount; return speed; } /** * Sets the target speed in "encoder ticks/16 ms interval" * */ void SetTargetSpeed(float target) { targetSpeed = target; if(ctrlMode != CTRL_SPEED) { // 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(bool debug = false) { if(ctrlMode == CTRL_SPEED) { /** * TODO: Implement integral, derivative control for the motors here! */ // Calculate the error in speed float error = targetSpeed - speed; sumError += error; float errorDiff = error - prevError; prevError = error; // Calculate the effort from the PID gains int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff + Kf * targetSpeed; // Set the effort for the motor SetEffort(effort); if(debug) { Serial.print(targetSpeed); Serial.print('\t'); Serial.print(speed); Serial.print('\t'); Serial.print(error); Serial.print('\t'); Serial.print(effort / 10.0); // N.B. that we divide by 10 to make the graph cleaner Serial.print('\n'); } } } static void AttachInterrupts(void); /** * InitializeMotorPWMTimer() should be called near the beginning of the program. * It sets up Timer4 to run at 38 kHz, which is used to both drive the PWM signal for the motors * and (tangentially) allow for a 38 kHz signal on pin 11, which can be used, say, to drive an * IR LED at a common rate. * * Timer 1 has the following configuration: * prescaler of 1 * outputs enabled on channels A (pin 9), B (pin 10) and C (pin 11) * fast PWM mode * top of 420, which will be the max speed * frequency is then: 16 MHz / [1 (prescaler) / (420 + 1)] = 38.005 kHz * */ static void InitializePWMTimerAndInterrupts(void) { Serial.println("InitMotor()"); noInterrupts(); //disable interrupts while we set Timer1 registers TCCR1A = 0xA2; //0b10100010; //Fast PWM + enable A and B; change to 0xAA to enable C on pin 11 TCCR1B = 0x19; //0b00011001; //Fast PWM ICR1 = 420; //runs at 38kHz; lowers speed for given effort by 5% from Pololu version //set all three outputs to zero OCR1A = 0; OCR1B = 0; OCR1C = 0; //can be used to create 38 kHz signal on pin 11; must enable output in TCCR1A above /** * Call a static function to set up the left and right motor interrupts */ AttachInterrupts(); interrupts(); //re-enable interrupts Serial.println("/InitMotor()"); } public: void SetPIDCoeffs(float p, float i, float d) {Kp = p; Ki = i; Kd = d; sumError = 0;} }; template class Romi32U4EncodedMotor : public Romi32U4MotorBase { protected: void InitializeMotor(void) { FastGPIO::Pin::setOutputLow(); FastGPIO::Pin::setOutputLow(); InitializeEncoder(); } /** * SetEffort is used internally to set the motor effort without changing the control mode. */ void SetEffort(int16_t effort) { bool reverse = 0; if (effort < 0) { effort = -effort; // Make speed a positive quantity. reverse = 1; // Reverse the direction. } if (effort > maxEffort) { effort = maxEffort; } FastGPIO::Pin::setOutput(reverse); _SFR_MEM16(OCR) = effort; } void InitializeEncoder(void) { Serial.println("InitEnc()"); // Set the pins as pulled-up inputs. FastGPIO::Pin::setInputPulledUp(); FastGPIO::Pin::setInputPulledUp(); // Initialize the variables so that the speed will start as 0 lastB = FastGPIO::Pin::isInputHigh(); lastA = FastGPIO::Pin::isInputHigh() ^ lastB; Serial.println("/InitEnc()"); } public: void ProcessEncoderTick(void) { bool newB = FastGPIO::Pin::isInputHigh(); bool newA = FastGPIO::Pin::isInputHigh() ^ newB; encCount += (lastA ^ newB) - (newA ^ lastB); lastA = newA; lastB = newB; } friend class Chassis; // Allow Chassis to call protected methods directly }; extern Romi32U4EncodedMotor leftMotor; extern Romi32U4EncodedMotor rightMotor;