#include <Servo.h>
//Set up pins
int motorSpeedPin_1 = 3; // pin for motor 1 demand
int motorDirPin_1 = 12; // pin for motor 1 direction
int motorBrakePin_1 = 9; // pin for motor 1 brake
int motorSpeedPin_2 = 11; // pin for motor 2 demand
int motorDirPin_2 = 13; // pin for motor 2 direction
int motorBrakePin_2 = 8; // pin for motor 2 brake
int encPhaseA_1 = 18; // pin for encoder 1 phase A
int encPhaseB_1 = 19; // pin for encoder 1 phase B
int encPhaseA_2 = 20; // pin for encoder 2 phase A
int encPhaseB_2 = 21; // pin for encoder 2 phase B
int blackButtonPin = A9; // pin for black button
int greenButtonPin = A10; // pin for green button
int potPin = A8; // pin for rotary pot
int linMotorPin = 6; // pin for linear motor
Servo linMotor;
//Set up variables
volatile int encCount_1 = 0; // Counts made for encoder 1
volatile int encCount_2 = 0; // Counts made for encoder 2
float encCountPerSec_1 = 0; // Encoder 1 angular velocity in counts/s
float encCountPerSec_2 = 0; // Encoder 2 angular velocity in counts/s
float motorDemand_1 = 0; // Motor 1 demand, -100% to +100%
float motorDemand_2 = 0; // Motor 2 demand, -100% to +100%
int motorCommand_1 = 0; // Motor 1 signal, 0 to 255
int motorCommand_2 = 0; // Motor 2 signal, 0 to 255
volatile int linMotorPos = 0; // Linear motor pulse length
int potValue; // Potentiometer value.
float maxPot = 270; // Used to calculate potentimeter value.
float potBias; // Potentiometer bias -1 to +1
unsigned long timePassed = 0; // Time in msec since buttom press
unsigned long timeStarted = 0; // Time is msec when button pressed
int buttonPressed = 0; // Which button Pressed, 0 = neither, 1 = black, 2 = green
int distTravlld = 0; // Distance buggy has travelled from start point in mm
int autoPilotAlogarithm = 1; // Informs on which alogaithm is active
float Time, ElapsedTime, SetupTime; // The actual Time, the actual Time subtracted from the previous Time, and the setup time.
float PriorTime = millis(); // Technically here for first milli
bool StartTimer = false; // Timer used for retraction
float SystemTime, TimePrev; // Time after setup till present
unsigned long PrevTime = millis(); // Increasing time needed for reasoning
double prevError = 0;
double integral = 0;
// Set up constants
const int zero = 0; // 0 in letter form
const double startZone = 0; // Distance buggy starts from in mm
const double dropZone = 6000; // Distance from start to drop zone in mm
const double endZone = 11000; // Distance from start to end zonevin mm
const int minButtonTime = 10000; // Minimum time button can be active for ????????????
const int maxButtonTime = 60000; // Maximum time button can be active for ????????????
const int retractTime = 5000; // Time taking to retract encoder ???????????????????
const int tyreDiameter = 176; // Tyre circumference in mm
const unsigned long Interval = 40; // loop period (in ms)
void setup() {
PINSETUP();
LINEARMOTORCONTROLSETUP();
BUTTONBOXSETUP();
INTERRUPTATTATCHMENTS();
SYSTEMCOMMS();
}
void PINSETUP() {
// Setup Motor 1
pinMode(motorDirPin_1, OUTPUT); // Initiates Motor Channel A pin
pinMode(motorBrakePin_1, OUTPUT); // Initiates Brake Channel A pin
// Setup Motor 2
pinMode(motorDirPin_2 , OUTPUT); // Initiates Motor Channel A pin
pinMode(motorBrakePin_2, OUTPUT); // Initiates Brake Channel A pin
// Setup Encoder 1
pinMode(encPhaseA_1, INPUT); // Initiates Encoder 1 Phase A
pinMode(encPhaseB_1, INPUT); // Initiates Encoder 1 Phase B
// Setup Encoder 2
pinMode(encPhaseA_2, INPUT); // Initiates Encoder 2 Phase A
pinMode(encPhaseB_2, INPUT); // Initiates Encoder 2 Phase B
}
void LINEARMOTORCONTROLSETUP() {
// Setup Linear Motor Control
linMotor.attach(6);
linMotor.writeMicroseconds(1000); // Change to 2000 to initialise fully extended
delay(3000); // Initiates the linear motor control pin
}
void BUTTONBOXSETUP() {
// Setup Button box
pinMode(blackButtonPin, INPUT); // Initiates Black Button
pinMode(greenButtonPin, INPUT); // Initiates Green Button
pinMode(potPin, INPUT); // Initiates Potentiometer
}
void INTERRUPTATTATCHMENTS() {
attachInterrupt(digitalPinToInterrupt(encPhaseA_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase A
attachInterrupt(digitalPinToInterrupt(encPhaseB_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase B
attachInterrupt(digitalPinToInterrupt(encPhaseA_2),encInc_2,CHANGE); // Create interrupt for change in encoder 2 phase A
attachInterrupt(digitalPinToInterrupt(encPhaseB_2),encInc_2,CHANGE); // Create interrupt for change in encoder 2 phase B
}
void SYSTEMCOMMS() {
Serial.begin(9600); // setup serial
SetupTime = millis(); // Retrieves time taken to Setup controller
}
// Main loop
void loop(){
CLOSEDLOOPPROGRAM(); // ClosedLoopProgram running at selected frequency via selected interval.
}
void CLOSEDLOOPPROGRAM() {
if (millis() - PrevTime >= Interval) { // 25Hz if enquiry
SYSTEMTIME();
CHANNELBRAKESENGAGEMENT();
BUTTONENQUIRY();
VARIABLEINITIALIZATION();
AUTOPILOTALGORITHM1();
AUTOPILOTALGORITHM2();
DRIVETRAINALGORITHM();
DEBUGGINGENQUIRY();
DEINITIALIZATION();
PrevTime += Interval; // Records new altering old PrevTime value.
} // End of 25Hz if enquiry
}
void SYSTEMTIME() {
TimePrev = Time; // The previous Time is stored before the actual Time read
Time = millis(); // Actual Time read
ElapsedTime = ( Time - TimePrev ); // Time that has elapsed
SystemTime = ( Time - SetupTime ); // The actual time converted from milliseconds to seconds
}
void CHANNELBRAKESENGAGEMENT() {
digitalWrite(motorBrakePin_1, HIGH); //Engage the Brake for Channel A
digitalWrite(motorBrakePin_2, HIGH); //Engage the Brake for Channel B
}
void BUTTONENQUIRY() {
// Initial buttonPressed variable.
buttonPressed = 0; //Set button pressed to 0
// Waits in this section until button pressed.
while (buttonPressed == 0) { //Stay in while loop until a button is pressed.
if (analogRead(blackButtonPin) < 2) { //Check for the black button to be pressed.
buttonPressed = 1; //Set button pressed to 1
}
else if (analogRead(greenButtonPin) < 2) { //Check for the green button to be pressed.
buttonPressed = 2; //Set button pressed to 2
}
else { //Else no button pressed.
buttonPressed = 0; //Set button pressed to 0
}
delay(50); //Wait 50ms before checking button status again.
}
}
void VARIABLEINITIALIZATION() {
// Code about to start running, intialise some variables ready for the running loop.
potValue = analogRead(potPin); // Read potentiometer value
potBias = (potValue-260)/maxPot; // Calculate potBias
encCount_1 = 0; // Initialise encoder 1
encCount_2 = 0; // Initialise encoder 2
timeStarted = millis(); // Get the time in sec at which the button was pressed.
timePassed = millis() - timeStarted;
}
void AUTOPILOTALGORITHM1() {
if ( autoPilotAlogarithm = 1 ) {
timePassed = millis() - timeStarted;
// Place your run code here:
PID1();
if (distTravlld >= dropZone) {
analogWrite(motorSpeedPin_1, zero); //Spins the motor on Channel A at motor demand
analogWrite(motorSpeedPin_2, zero); //Spins the motor on Channel B at motor demand
delay(2);
}
}
}
void AUTOPILOTTRANSFER() {
if ( ( distTravlld >= dropZone ) && ( encCountPerSec_1 == zero ) && ( encCountPerSec_2 == zero ) && ( StartTimer == false ) ) {
StartTimer = true; // Sets a value used in autopilot transfer void
StartTimer = true; // Sets a value used in Scan Stop Enquiry
PriorTime = SystemTime; // Sets a value used in Scan Stop Enquiry
linMotor.writeMicroseconds(1000); // Change to 2000 to extend, 1000 to retract
delay (50);
}
if ( ( StartTimer == true ) && ( SystemTime >= ( PriorTime + retractTime ) ) ) {
autoPilotAlogarithm = 2; // Transfers autopilot from 1 to 2
delay(2);
}
}
void AUTOPILOTALGORITHM2() {
if ( autoPilotAlogarithm = 2 ) {
timePassed = millis() - timeStarted;
// Place your run code here:
PID2();
if (distTravlld >= endZone) {
analogWrite(motorSpeedPin_1, zero); //Spins the motor on Channel A at motor demand
analogWrite(motorSpeedPin_2, zero); //Spins the motor on Channel B at motor demand
delay(2);
}
}
}
void DRIVETRAINALGORITHM() {
// This code below is used to output the demand to the motors, it should not need to be changed.
motorCommand_1 = round(motorDemand_1*2.55); //Coverts -100 to 100 to -255 to 255
motorCommand_2 = round(motorDemand_2*2.55); //Coverts -100 to 100 to -255 to 255
motorCommand_1 = abs(motorCommand_1); //Makes motorCommand_1 a positive value.
motorCommand_2 = abs(motorCommand_2); //Makes motorCommand_2 a positive value.
if (motorDemand_1 > 0) { //Sets motor 1 direction.
digitalWrite(motorDirPin_1, HIGH); //Establishes forward direction of Channel A
}
else {
digitalWrite(motorDirPin_1, LOW); //Establishes reverse direction of Channel A
}
digitalWrite(motorBrakePin_1, LOW); //Disengage the Brake for Channel A
analogWrite(motorSpeedPin_1, motorCommand_1); //Spins the motor on Channel A at motor demand
if (motorDemand_2 > 0) { //Sets motor 2 direction.
digitalWrite(motorDirPin_2, HIGH); //Establishes forward direction of Channel B
}
else {
digitalWrite(motorDirPin_2, LOW); //Establishes reverse direction of Channel B
}
digitalWrite(motorBrakePin_2, LOW); //Disengage the Brake for Channel B
analogWrite(motorSpeedPin_2, motorCommand_2); //Spins the motor on Channel B at motor demand
delay(2);
}
void DEBUGGINGENQUIRY() {
// For debugging, remove once code is complete
Serial.print("Time passed: ");
Serial.print(timePassed);
Serial.print(" Encoder 1: ");
Serial.print(encCount_1);
Serial.print(" Encoder 2: ");
Serial.print(encCount_2);
Serial.print(" Encoder count per sec 1: ");
Serial.print(analogRead(blackButtonPin));
Serial.print(" Encoder count per sec 2: ");
Serial.print(analogRead(greenButtonPin));
Serial.print(" MotorDemand_1: ");
Serial.print(motorDemand_1);
Serial.print(" MotorDemand_2: ");
Serial.print(motorDemand_2);
Serial.println();
}
void DEINITIALIZATION() {
if ( ( distTravlld >= endZone ) || ( timePassed >= maxButtonTime ) ) {
exit(0);
}
}
void PID1() {
//PID constants
double kp = 1.0;
double ki = 0.1;
double kd = 0.01;
//calculate error
double error = dropZone - distTravlld;
//calculate integral
integral += error;
//calculate derivative
double derivative = error - prevError;
//calculate output
double output = kp*error + ki*integral + kd*derivative;
//update previous error
prevError = error;
}
void PID2() {
//PID constants
double kp = 1.0;
double ki = 0.1;
double kd = 0.01;
//calculate error
double error = dropZone - distTravlld;
//calculate integral
integral += error;
//calculate derivative
double derivative = error - prevError;
//calculate output
double output = kp*error + ki*integral + kd*derivative;
//update previous error
prevError = error;
}
// Function for calculating encoder velocity 1
void encVel_1() {
int measurementWidth = 4; // Number of counts that need to pass before determining velocity
static int timeElapsed = 0; // Time elasped since last velocity given
static int refTime; // Current reference time in ms
static float encChange =0; // Change in encoder count since last velocity measure
static int prevEncCount = 0; // Encoder value at last velocity measure
static int encChangeAbs = 0; // Absolute change in encoder count
static int storedTime = millis(); // Time at previous velocity measure in ms
refTime = millis(); // Update reference time in ms
timeElapsed = refTime-storedTime; // Calcualte the time that has elapsed since last velocity measure
encChange = encCount_1-prevEncCount; // Calculate the change in counts since previous velocity measure
if (timeElapsed >= 400) { // If time is over 400ms since last velocity measure, assume zero velocity
encCountPerSec_1 = 0;
prevEncCount = encCount_1; // set previous encoder Count to the current encoder count and storedTime to current time in ms
storedTime = refTime;
}
else { // If the time elapsed is short...
encChangeAbs = abs(encChange); // Get absolute encoder change.
if (encChangeAbs >= measurementWidth) { // If absolute encoder change is greater or equal to measurement width...
encCountPerSec_1 = 1000*(encChange/timeElapsed); // Calculate counts per second to give velocity measure
storedTime = refTime; // set previous encoder Count to the current encoder count and storedTime to current time in ms
prevEncCount = encCount_1;
}
}
}
// Function for calculating encoder velocity 2
void encVel_2() {
int measurementWidth = 4; // Number of counts that need to pass before determining velocity
static int timeElapsed = 0; // Time elasped since last velocity given
static int refTime; // Current reference time in ms
static float encChange =0; // Change in encoder count since last velocity measure
static int prevEncCount = 0; // Encoder value at last velocity measure
static int encChangeAbs = 0; // Absolute change in encoder count
static int storedTime = millis(); // Time at previous velocity measure in ms
refTime = millis(); // Update reference time in ms
timeElapsed = refTime-storedTime; // Calcualte the time that has elapsed since last velocity measure
encChange = encCount_2-prevEncCount; // Calculate the change in counts since previous velocity measure
if (timeElapsed >= 400) { // If time is over 400ms since last velocity measure, assume zero velocity
encCountPerSec_2 = 0;
prevEncCount = encCount_2; // set previous encoder Count to the current encoder count and storedTime to current time in ms
storedTime = refTime;
}
else { // If the time elapsed is short...
encChangeAbs = abs(encChange); // Get absolute encoder change.
if (encChangeAbs >= measurementWidth) { // If absolute encoder change is greater or equal to measurement width...
encCountPerSec_2 = 1000*(encChange/timeElapsed); // Calculate counts per second to give velocity measure
storedTime = refTime; // set previous encoder Count to the current encoder count and storedTime to current time in ms
prevEncCount = encCount_2;
}
}
}
// ISR for counting the encoder 1 value
void encInc_1() {
static bool prevPhaseA;
static bool prevPhaseB;
bool currentPhaseA;
bool currentPhaseB;
currentPhaseA = digitalRead(encPhaseA_1); // Get current value of phase A and phase B
currentPhaseB = digitalRead(encPhaseB_1);
if (currentPhaseA != prevPhaseA) { // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B
if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) {
encCount_1 ++;
}
else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) {
encCount_1 --;
}
else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) {
encCount_1 --;
}
else {
encCount_1 ++;
}
}
else if (currentPhaseB != prevPhaseB) { // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B
if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) {
encCount_1 --;
}
else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) {
encCount_1 ++;
}
else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) {
encCount_1 ++;
}
else {
encCount_1 --;
}
}
prevPhaseA = currentPhaseA;
prevPhaseB = currentPhaseB;
}
// ISR for counting the encoder 2 value
void encInc_2() {
static bool prevPhaseA;
static bool prevPhaseB;
bool currentPhaseA;
bool currentPhaseB;
currentPhaseA = digitalRead(encPhaseA_2); // Get current value of phase A and phase B
currentPhaseB = digitalRead(encPhaseB_2);
if (currentPhaseA != prevPhaseA) { // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B
if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) {
encCount_2 ++;
}
else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) {
encCount_2 --;
}
else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) {
encCount_2 --;
}
else {
encCount_2 ++;
}
}
else if (currentPhaseB != prevPhaseB) { // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B
if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) {
encCount_2 --;
}
else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) {
encCount_2 ++;
}
else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) {
encCount_2 ++;
}
else {
encCount_2 --;
}
}
prevPhaseA = currentPhaseA;
prevPhaseB = currentPhaseB;
}