Speed Control Class

The speed control class is what enables the robot to mantain a constant speed regardless of the incline at which the robot is driving on. It does so by taking information from the wheel encoders and periodically updating registery information in the Arduino.

.cpp Code

#include <SpeedControl.h>

SpeedControl::SpeedControl(HardwareSerial *port, Encoder *encoderPtr, Motor *motorPtr1, Motor *motorPtr2, double sampleTime){
	m_setPoint = 0.0;
	m_enable = 0;
	m_error = 0.0;
	m_currentSpeed = 0.0;
	m_prevSpeed = 0.0;
	m_errorIntegral = 0.0;
	m_sampleTime = sampleTime;
	m_motor1 = motorPtr1;
	m_motor2 = motorPtr2;
	m_encoder = encoderPtr;
	m_gainIntegral = 100.0;
	m_gainProportional = 10.0;
	m_gainDifferential = 25.0;
	m_integratorLimit = 50.0;
	m_serialPort = port;
}

void SpeedControl::setDesiredSpeed(double speed){
	m_setPoint = speed;
}

double SpeedControl::getDesiredSpeed(){
	return(m_setPoint);
}

	void SpeedControl::enable(){
	m_enable = 1;
}

void SpeedControl::disable(){
	m_enable = 0;

void SpeedControl::update(double speed){
	m_prevSpeed = m_currentSpeed;
	m_currentSpeed = speed;
	m_error = (m_currentSpeed - m_setPoint)/100;
	m_errorIntegral += m_error*m_sampleTime;

	if(m_errorIntegral > m_integratorLimit)
		m_errorIntegral = m_integratorLimit;

	else if(m_errorIntegral < -m_integratorLimit)
		m_errorIntegral = -m_integratorLimit;
		double pTerm = m_gainProportional*m_error;
		double iTerm = m_gainIntegral*m_errorIntegral;
		double dTerm = m_gainDifferential *(m_currentSpeed - m_prevSpeed)/m_sampleTime;
		double newDrive = -(iTerm + pTerm - dTerm);
		newDrive = 255.0*newDrive/30.0;

	if(newDrive < 0.0)
		newDrive = 0.0;

	else if(newDrive > 255.0)
		newDrive = 255.0;

	if(m_enable){
		if(newDrive == 0.0){
			m_motor1 -> setDrive(0, RELEASE);
			m_motor2 -> setDrive(0, RELEASE);
		}

		else{
			m_motor1 -> setDrive((int)newDrive, FORWARD);
			m_motor2 -> setDrive((int)newDrive, FORWARD);
		}

		m_serialPort -> print(m_error);
		m_serialPort -> print(", ");
		m_serialPort -> print(m_errorIntegral);
		m_serialPort -> print(", ");
		m_serialPort -> print(newDrive/255.0);// Normalize
		m_serialPort -> print("\r");
	}

}

.h Code

#include "Arduino.h"
#include <Encoder.h>
#include <Motor.h>
#ifndef SpeedControl_h
#define SpeedControl_h

class SpeedControl{
	
private:
	double m_setPoint;
	double m_currentSpeed;
	double m_prevSpeed;
	double m_sampleTime;
	double m_error;
	double m_errorIntegral;
	double m_gainProportional;
	double m_gainIntegral;
	double m_gainDifferential;
	double m_integratorLimit;
	bool m_enable;
	Encoder *m_encoder;
	Motor *m_motor1;
	Motor *m_motor2;
	HardwareSerial *m_serialPort;
	
public:
	SpeedControl(HardwareSerial *port, Encoder *encoderPtr, Motor *motorPtr1, Motor *motorPtr2, double sampleTime);
	void setDesiredSpeed(double speed);
	double getDesiredSpeed();
	void enable();
	void disable();
	void update(double speed);
	void setPGain(double gain);
	void setIGain(double gain);
	void setDGain(double gain);
	void setILimit(double limit);

};

#endif