#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; // Time is msec when button pressed.
unsigned long prevTime_1 = 0; // Time in msec of the last loop
unsigned long prevTime_2 = 0; // Time in msec of the last loop
int buttonPressed = 0; // Which button Pressed, 0 = neither, 1 = black, 2 = green
int encCountPerRev = 512; // Number of counts per revolution
int velocity_1 = 0; // Encoder 1 Wheel Speed
int velocity_2 = 0; // Encoder 2 Wheel Speed
int avg_distanceTravelled = 0; // Average distance travelled by the buggy
int distanceTravelled_1 = 0; // Calculated distance by encoder 1
int distanceTravelled_2 = 0; // Calculated distance by encoder 2
int wheelDiameter = 176; // Used to calculate wheel velocity and distance travelled
bool dropZone = false; // Intialise payload logic
static int dt = 0; // Controller frequency in msecs
float prevError_1 = 0; // Initalise previous error values
float prevError_2 = 0; // Initalise previous error values
int targetVelocity = 70; // Initalise target velocity of the wheels
float controlSignal_1 = 0; // Intialise the Control signal value
float controlSignal_2 = 0; // Intialise the Control signal value
int outMax = 100; // Intialise integral output values
int outMin = -100; // Intialise integral output values
void setup() {
// Setup the pins and variables for the programme and define the interupts for the encoder.
// 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
// Setup Linear Motor Control
linMotor.attach(6);
linMotor.writeMicroseconds(2000); // Change to 2000 to initialise fully extended
delay(1000); // Initiates the linear motor control pin
// Setup Button box
pinMode(blackButtonPin, INPUT); // Initiates Black Button
pinMode(greenButtonPin, INPUT); // Initiates Green Button
pinMode(potPin, INPUT); // Initiates Potentiometer
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
Serial.begin(9600); // setup serial
Serial.print(" 1. System Start ");
}
// Main loop
void loop(){
digitalWrite(motorBrakePin_1, HIGH); //Engage the Brake for Channel A
digitalWrite(motorBrakePin_2, HIGH); //Engage the Brake for Channel B
delay(3000);
// 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.
}
Serial.print(" 2. Button Pressed" );
// 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; // Initialise timePassed to 0
if (buttonPressed == 2) { //Black button pushed for simple controller
while (avg_distanceTravelled < 11500) {
distanceTravelled_1 = -1*(encCount_1/encCountPerRev)*wheelDiameter*PI; // Calculation for the distance travelled by each wheel
distanceTravelled_2 = (encCount_2/encCountPerRev)*wheelDiameter*PI; // Calculation for the distance travelled by each wheel
avg_distanceTravelled = (distanceTravelled_1 + distanceTravelled_2)/2; // Average distance travelled between both wheels
if (avg_distanceTravelled < 5500) { // Motor control for the intial phase
motorDemand_1 = -80;
motorDemand_2 = 80;
}
else if (avg_distanceTravelled >= 5500 && avg_distanceTravelled <= 6900) { // Motor control during Payload drop phase
motorDemand_1 = -20;
motorDemand_2 = 20;
}
else if (avg_distanceTravelled > 6900 && avg_distanceTravelled <= 11500) { // Motor control for final phase
motorDemand_1 = -80;
motorDemand_2 = 80;
}
else {
motorDemand_1 = 0;
motorDemand_2 = 0;
}
encVel_1(); // call encoder velcoity functions
encVel_2();
motorControl(); // call motor control function
payloadDrop();
Serial.print(" motor demand 1 ");
Serial.print(motorDemand_1);
Serial.print(" motor demand 2 ");
Serial.print(motorDemand_2);
Serial.print(" distance travelled ");
Serial.print(avg_distanceTravelled);
Serial.print(" Wheel speed 1 ");
Serial.print(velocity_1);
Serial.print(" Wheel speed 2 ");
Serial.print(velocity_2);
Serial.println();
}
motorCommand_1 = 0; // Slow down buggy once it reaches the finish line
motorCommand_2 = 0;
}
if (buttonPressed == 1) { //Green putton pushed run PID Controller
// This is the run loop, you set the condition for the while loop to keep running
while (avg_distanceTravelled < 11500) { //Add your condition for the loop to continue running here, that is when should your controller stop?
timePassed = millis() - timeStarted;
distanceTravelled_1 = -1*(encCount_1/encCountPerRev)*wheelDiameter*PI;
distanceTravelled_2 = (encCount_2/encCountPerRev)*wheelDiameter*PI;
avg_distanceTravelled = (distanceTravelled_1 + distanceTravelled_2)/2;
// Place your run code here:
encVel_1();
encVel_2();
//Check what the target velcoity should be depending where the buggy is
targetVelocitycheck();
//Check if drop zone
payloadDrop();
//Calculate control signal
calculatePID_1();
calculatePID_2();
// 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);
// 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(encCountPerSec_1);
Serial.print(" Encoder count per sec 2: ");
Serial.print(encCountPerSec_2);*/
Serial.print(" MotorDemand_1: ");
Serial.print(motorDemand_1);
Serial.print(" MotorDemand_2: ");
Serial.print(motorDemand_2);
Serial.print(" Average Distance Travelled ");
Serial.print(avg_distanceTravelled);
Serial.print(" Wheel 1 Distance ");
Serial.print(distanceTravelled_1);
Serial.print(" Wheel 2 Distance ");
Serial.print(distanceTravelled_1);
Serial.print(" Wheel speed 1 ");
Serial.print(velocity_1);
Serial.print(" Wheel speed 2 ");
Serial.print(velocity_2);
Serial.println();
}
} motorCommand_1 = 0; // Slow down buggy once it reaches the finish line
motorCommand_2 = 0;
}
// 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;
}
}
velocity_1 = (encCountPerSec_1 / encCountPerRev)*60; //Calculate the rpm of the wheel 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;
}
}
velocity_2 = (encCountPerSec_2 / encCountPerRev)*60; //Calculate the rpm of the wheel 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;
}
//Function for calculating motor demand signal
void calculatePID_1() {
int kp = 1.2;
int ki = 2.5;
int kd = 0;
int velocityError_1 = 0;
static float integral_1;
float derivative_1 = 0;
static int dt = 0; // Time elasped since last velocity given
static int refTime; // Current reference time in ms
static int storedTime = millis(); // Time at previous velocity measure in ms
refTime = millis(); // Update reference time in ms
dt = refTime-storedTime;
storedTime = millis();
velocityError_1 = -targetVelocity - velocity_1; //calculate velocity error for wheel 1
integral_1 += velocityError_1*dt/1000;
if (integral_1 < outMin) { //restricts intergral term to reduce windup
integral_1 = outMin;
}
else if (integral_1 > outMax) {
integral_1 = outMax;
}
derivative_1 = (velocityError_1 - prevError_1)/dt;
prevError_1 = velocityError_1;
controlSignal_1 = kp*velocityError_1 + ki*integral_1 + kd*derivative_1;
motorDemand_1 = constrain(controlSignal_1,-100,100);
}
void calculatePID_2() {
int kp = 1.2;
int ki = 2.5;
int kd = 0;
int velocityError_2 = 0;
static float integral_2;
float derivative_2 = 0;
static int dt = 0; // Time elasped since last velocity given
static int refTime; // Current reference time in ms
static int storedTime = millis(); // Time at previous velocity measure in ms
refTime = millis(); // Update reference time in ms
dt = refTime-storedTime;
storedTime = millis();
velocityError_2 = targetVelocity - velocity_2; // calculate velocity error for wheel 2
integral_2 += velocityError_2*dt/1000;
if (integral_2 > outMax) { //restricts intergral term to reduce windu
integral_2 = outMax;
}
else if (integral_2 < outMin) {
integral_2 = outMin;
}
derivative_2 = (velocityError_2 - prevError_2)/dt;
prevError_2 = velocityError_2;
controlSignal_2 = (kp*velocityError_2 + ki*integral_2);
motorDemand_2 = constrain(controlSignal_2,-100,100);
}
void payloadDrop () {
if (avg_distanceTravelled >= 6075 && avg_distanceTravelled <= 6825) {
dropZone = true;
}
else {
dropZone = false;
}
if (dropZone == true) {
linMotor.writeMicroseconds(1000);
}
else {
linMotor.writeMicroseconds(2000);
}
}
void targetVelocitycheck () {
if (avg_distanceTravelled >= 5500 && avg_distanceTravelled <= 6850) {
targetVelocity = 10;
}
else if (avg_distanceTravelled >= 6850 && avg_distanceTravelled <= 11500) {
targetVelocity = 80;
}
else {
targetVelocity = 80;
}
}
void motorControl() {
// 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 AllignmentCheck() {
potValue = analogRead(potPin);
potBias = (potValue-260)/maxPot;
float diffVel = 0;
float magVel = 0;
float motorBias_1 = 0;
float motorBias_2 = 0;
if (velocity_1 > velocity_2) {
diffVel = velocity_1 - velocity_2;
magVel = velocity_1 + velocity_2;
motorBias_1 = 1 - diffVel/magVel;
motorBias_2 = 1;
}
else if (velocity_2 > velocity_1) {
diffVel = velocity_2 - velocity_1;
magVel = velocity_1 + velocity_2;
motorBias_2 = 1 - diffVel/magVel;
motorBias_1 = 1;
}
else {
motorBias = 1;
}
}*/