diff --git a/EncTest/EncTest.ino b/EncTest/EncTest.ino new file mode 100644 index 0000000..9f52b39 --- /dev/null +++ b/EncTest/EncTest.ino @@ -0,0 +1,63 @@ +#include "digitalIOPerformance.h" +byte previousState = 0x00; +void setup() { + pinModeFast(18,INPUT); + pinModeFast(19,INPUT); + byte chanA = digitalReadFast(18); + byte chanB = digitalReadFast(19); + //previousState = ((~chanB)<<1)|((chanA&chanB)|(~(chanA|chanB))); //0-3 + if(~chanA&chanB) + previousState = 0x00; + else if(chanA&chanB) + previousState = 0x01; + else if(chanA&(~chanB)) + previousState = 0x02; + else + previousState = 0x03; + Serial.begin(115200); + interrupts(); + attachInterrupt(digitalPinToInterrupt(18), leftEncInterrupt, CHANGE); + attachInterrupt(digitalPinToInterrupt(19), leftEncInterrupt, CHANGE); +} + +static long count = 0; +static long lastCount = 0; +void loop() { + lastCount = count; + delay(1000); + Serial.println(count-lastCount); +} +void leftEncInterrupt() { + byte chanA = digitalReadFast(18); + byte chanB = digitalReadFast(19); + byte state = 0x00; + //byte state = 0x00; + if(~chanA&chanB) + state = 0x00; + else if(chanA&chanB) + state = 0x01; + else if(chanA&(~chanB)) + state = 0x02; + else + state = 0x03; + //byte state = ((~chanB)<<1)|((chanA&chanB)|(~(chanA|chanB))); //0-3 + if((previousState == 0x03) && (state==0x00)) + { + //wrapped around + count++; + } + else if((previousState == 0x00) && (state==0x03)) + { + //wrapped around + count--; + } + else if(state>previousState) + { + count++; + } + else if(statemax_val) + { + max_val = readings[k]; + } + if(readings[k] + +int IRsensorAddress = 0xB0; +//int IRsensorAddress = 0x58; +int slaveAddress; +int ledPin = 13; +boolean ledState = false; +byte data_buf[16]; +int i; + +int Ix[4]; +int Iy[4]; +int s; + +void Write_2bytes(byte d1, byte d2) +{ + Wire.beginTransmission(slaveAddress); + Wire.write(d1); Wire.write(d2); + Wire.endTransmission(); +} + +void setup() +{ + slaveAddress = IRsensorAddress >> 1; // This results in 0x21 as the address to pass to TWI + Serial.begin(19200); + pinMode(ledPin, OUTPUT); // Set the LED pin as output + Wire.begin(); + // IR sensor initialize + Write_2bytes(0x30,0x01); delay(10); + Write_2bytes(0x30,0x08); delay(10); + Write_2bytes(0x06,0x90); delay(10); + Write_2bytes(0x08,0xC0); delay(10); + Write_2bytes(0x1A,0x40); delay(10); + Write_2bytes(0x33,0x33); delay(10); + delay(100); +} +void loop() +{ + ledState = !ledState; + if (ledState) { digitalWrite(ledPin,HIGH); } else { digitalWrite(ledPin,LOW); } + + //IR sensor read + Wire.beginTransmission(slaveAddress); + Wire.write(0x36); + Wire.endTransmission(); + + Wire.requestFrom(slaveAddress, 16); // Request the 2 byte heading (MSB comes first) + for (i=0;i<16;i++) { data_buf[i]=0; } + i=0; + while(Wire.available() && i < 16) { + data_buf[i] = Wire.read(); + i++; + } + + Ix[0] = data_buf[1]; + Iy[0] = data_buf[2]; + s = data_buf[3]; + Ix[0] += (s & 0x30) <<4; + Iy[0] += (s & 0xC0) <<2; + + Ix[1] = data_buf[4]; + Iy[1] = data_buf[5]; + s = data_buf[6]; + Ix[1] += (s & 0x30) <<4; + Iy[1] += (s & 0xC0) <<2; + + Ix[2] = data_buf[7]; + Iy[2] = data_buf[8]; + s = data_buf[9]; + Ix[2] += (s & 0x30) <<4; + Iy[2] += (s & 0xC0) <<2; + + Ix[3] = data_buf[10]; + Iy[3] = data_buf[11]; + s = data_buf[12]; + Ix[3] += (s & 0x30) <<4; + Iy[3] += (s & 0xC0) <<2; + + for(i=0; i<4; i++) + { + if (Ix[i] < 1000) + Serial.print(""); + if (Ix[i] < 100) + Serial.print(""); + if (Ix[i] < 10) + Serial.print(""); + Serial.print( int(Ix[i]) ); + Serial.print(","); + if (Iy[i] < 1000) + Serial.print(""); + if (Iy[i] < 100) + Serial.print(""); + if (Iy[i] < 10) + Serial.print(""); + Serial.print( int(Iy[i]) ); + if (i<3) + Serial.print(","); + } + Serial.println(""); + delay(100); +} + diff --git a/LabFour/LabFour.ino b/LabFour/LabFour.ino new file mode 100644 index 0000000..6f2a8ef --- /dev/null +++ b/LabFour/LabFour.ino @@ -0,0 +1,127 @@ +//Written by Joseph St. Germain for RBE 2002 Lab 3 A16. +//Students are expected to fill in the PID::calc function and the calcCur function +// The PID calc function is inteded to run a PID velocity controller +//and the calcCur function should calculate the current in ma from the current sensor. + +#include "LookupTable.h" +#include "PID.h" +#include +#include + +//Setup lookup table class as lk +Lookup lk; +//setup PID class as pidCon +PID pidCon; +Encoder myEnc(2, 3); + +//Variables to run the code +//setVel is the velocity used as an input to PID +double setVel=90; +double curTime=0; +double prevTime=0; +double timeInterval=0; +double movement=0; +double ticksToDeg= 0.2206; +double prevPos; +long lastTime=0; +double Vel; +float velocity; +float RPM; +float current; + +// Returns Vel in degress/second +double calcVel(){ + //current positions + double curPos=myEnc.read(); + //current time + curTime=millis(); + //time change in ms from last call + timeInterval=curTime-prevTime; + //encoder ticks since last call + movement=curPos-prevPos; + //encoder ticks to degrees + movement= movement *ticksToDeg; + //timeInterval in seconds + timeInterval=timeInterval/1000; + //Velocity in degrees per milliseconds + Vel=movement/timeInterval; + //sets curent vals to previous + prevPos=curPos; + prevTime=curTime; + return Vel; +} + + +//This function should analogRead the current sense from the motor driver +//and convert the value to current in milliamps +double calcCur(void) +{ + //Read analog pin from current sensor + int val = analogRead(A1); + //convert to volts + float voltage = val*(5/1023.0f); + //converts to current in MilliAmps + float milliamps = (voltage/0.525)*1000; + return milliamps; +} + +void setup() { + Serial.begin(115200); + pidCon.setpid(2,0,.01); +} + + +//Main loop +void loop() { + +//Waits for the time constant +if(millis()-lastTime>10){ + + //Calculate velocity from the encoder in degrees/second + velocity=calcVel(); + //Serial.print("velocity="); + Serial.print(velocity); + //Serial.print(" , "); + Serial.print('\t'); + + //Calculates RPM from velocity + RPM =((velocity*60)/360); + //Serial.print("RPM = "); + Serial.print(RPM); + //Serial.print(" , "); + Serial.print('\t'); + + //calculates the current of the motor from the analogRead function + //use the motor drivers data sheet to find the conversion from ADC counts + //to milliamps + current = calcCur(); + //Serial.print("current = "); + Serial.print(current); + //Serial.print(" , "); + Serial.print('\t'); + + //Calculate the PID value to be sent to the motor using the analogWrite function. + //The calc function should take in a set velocity and the current velocity. + float analogWriteVal = pidCon.calc(setVel,velocity); + analogWrite(5,analogWriteVal); + + //Calculates the dutycycle of the PWM 0-1 + float PWM = analogWriteVal/255; +// Serial.print("PWM = "); + Serial.print(PWM); + // Serial.print(" , "); + Serial.print('\t'); + + //Use the lookup table class to find the torque given the current and RPM + //Serial.print("torque = "); + Serial.println(lk.torque(current,RPM)); + + + + + + //Sets the lastTime variable + lastTime=millis(); +} +} + diff --git a/LabFour/LookupTable.cpp b/LabFour/LookupTable.cpp new file mode 100644 index 0000000..7c80ad0 --- /dev/null +++ b/LabFour/LookupTable.cpp @@ -0,0 +1,112 @@ +/* + * Messages.h + * + * Created on: 10/1/16 + * Author: joest + */ + +#include "LookupTable.h" +#include "Arduino.h" + +//array of torques based off current and RPM. +//Review the provided excel document for a better understanding. +const float torqueArray[11][11]= +{ +{0,0,0,0,0,0,0,0,0,0,0}, +{0,0,0,0,0,0,0,0,0,0,6.3}, +{0,0,0,0,0,0,2.52,5.04,7.56,10.08,12.6}, +{0,0,0,0,2.7,5.4,8.1,10.8,13.5,16.2,18.9}, +{0,0,0,3.15,6.3,9.45,12.6,15.75,18.9,22.05,25.2}, +{0,0,3.5,7,10.5,14,17.5,21,24.5,28,31.5}, +{0,0,4.2,8.4,12.6,16.8,21,25.2,29.4,33.6,37.8}, +{0,0,4.9,9.8,14.7,19.6,24.5,29.4,34.3,39.2,44.1}, +{0,5.04,10.08,15.12,20.16,25.2,30.24,35.28,40.32,45.36,50.4}, +{0,5.67,11.34,17.01,22.68,28.35,34.02,39.69,45.36,51.03,56.7}, +{0,6.3,12.6,18.9,25.2,31.5,37.8,44.1,50.4,56.7,63} + +}; + +//Class constructor +Lookup::Lookup(){ + +} + +//Function that finds the row and colum of the torque based +//off the current in milliamps and the RPM +float Lookup::torque(float current,float rpm) +{ + int x =0; + int y =0; + // Sets the row based on current reading + if( current>=0 && current<219){ + x=0; + } + if( current>220 && current<439){ + x=1; + } + if( current>440 && current<659){ + x=2; + } + if( current>660 &¤t<879){ + x=3; + } + if( current>880 && current<1099){ + x=4; + } + if(current>1100 && current<1319){ + x=5; + } + if( current>1320 && current<1539){ + x=6; + } + if( current>1540 && current<1759){ + x=7; + } + if( current>1760 && current<1979){ + x=8; + } + if ( current>1980 && current<2199){ + x=9; + } + if( current>2200 && current<3000){ + x=10; + } + + //Sets the column based off RPM + if(rpm<300 && rpm>200){ + y=0; + } + if(rpm<199 && rpm>180){ + y=1; + } + if(rpm<179 && rpm>160){ + y=2; + } + if(rpm<159 && rpm>140){ + y=3; + } + if(rpm<139 && rpm>120){ + y=4; + } + if(rpm<119 && rpm>100){ + y=5; + } + if(rpm<99 && rpm>80){ + y=6; + } + if(rpm<79 && rpm>60){ + y=7; + } + if(rpm<59 && rpm>40){ + y=8; + } + if(rpm<39 && rpm>20){ + y=9; + } + if(rpm<19 && rpm>0){ + y=10; + } + //returns the torque from the lookup table + float torq=torqueArray[x][y]; + return torq; + } diff --git a/LabFour/LookupTable.h b/LabFour/LookupTable.h new file mode 100644 index 0000000..f50130f --- /dev/null +++ b/LabFour/LookupTable.h @@ -0,0 +1,22 @@ +/* + * Messages.h + * + * Created on: 10/1/16 + * Author: joest + */ + +#ifndef Lookup_H +#define Lookup_H + +//lookup table class +class Lookup { + public: + Lookup(); + //lookup torque function + float torque(float current,float rpm); + + private: + +}; + +#endif diff --git a/LabFour/PID.cpp b/LabFour/PID.cpp new file mode 100644 index 0000000..e848688 --- /dev/null +++ b/LabFour/PID.cpp @@ -0,0 +1,40 @@ +/* + * Messages.h + * + * Created on: 10/1/16 + * Author: joest + */ +#include "PID.h" +#include "Arduino.h" + +//Class constructor +PID::PID(){ + +} + +//Function to set PID gain values +void PID::setpid(float P, float I, float D){ + kp=P; + ki=I; + kd=D; +} + +//Write this function to calculate a control signal from the set velocity +//and the current velocity +float PID::calc(double setVel, double curVel){ + + // calculate error + float error = setVel-curVel; + // calculate derivative of error + float delta_error = last_error - error; + last_error = error; + // calculate integral error + sum_error += error; + // sum up the error value to send to the motor based off gain values. + float control_input = (kp*error) + (ki*sum_error) + (kd*delta_error); + + // limit control value to 0-254 + control_input = constrain(control_input,0,254); + //return the control signal + return control_input; +} diff --git a/LabFour/PID.h b/LabFour/PID.h new file mode 100644 index 0000000..d8d4127 --- /dev/null +++ b/LabFour/PID.h @@ -0,0 +1,31 @@ +/* + * Messages.h + * + * Created on: 10/1/16 + * Author: joest + */ + +#ifndef PID_H_ +#define PID_H_ + +class PID { + +public: +PID(); +float kp; +float ki; +float kd; +float last_error = 0; +float sum_error =0; +//set PID constants +void setpid(float P, float I, float D); +//calculate the output control signal +float calc(double setVel, double curVel); + +private: + +}; + + + +#endif diff --git a/LibPID/LibPID.ino b/LibPID/LibPID.ino new file mode 100644 index 0000000..aebf23d --- /dev/null +++ b/LibPID/LibPID.ino @@ -0,0 +1,63 @@ +#include +Encoder myEnc(18, 19); +#include ; +double v_target = 8; // in/sec PID setpoint +double motorVelocity = 0; //pid input +double motorPwr = 0; //pid output +double kp = 2000; +double ki = 6000; +double kd = 30; +PID myPID(&motorVelocity, &motorPwr, &v_target, kp, ki, kd, DIRECT); +void setup() { + Serial.begin(115200); + myPID.SetMode(AUTOMATIC); + myPID.SetSampleTime(25); + myPID.SetOutputLimits(10000,65535); + // put your setup code here, to run once: + initDrivePWM(); + pinMode(4,OUTPUT); + digitalWrite(4,HIGH); +} + +void loop() { + myPID.Compute(); + setDrivePWM(motorPwr); + updateVelocity(); + Serial.print(millis()); + Serial.print('\t'); + Serial.print(motorPwr); + Serial.print('\t'); + Serial.println(motorVelocity); +} +unsigned long lastCount = 0; +unsigned long long lastTime = 0; +static const float DIST_PER_REV = 3.14159*2.75; +static const float TICKS_PER_REV = 8245.81; +void updateVelocity() { + unsigned long diffCnt = myEnc.read()-lastCount; + unsigned long long diffTime = micros()-lastTime; + motorVelocity = (diffCnt*DIST_PER_REV*1000000.0)/(diffTime*TICKS_PER_REV); + lastTime = micros(); + lastCount = myEnc.read(); +} +#define PWM_FREQ 16000 +#define CLOCK_FREQ 16000000 +static const unsigned int PWM_COUNT = (((CLOCK_FREQ/PWM_FREQ))-1);// make constant to avoid recomputing each time + + +static void initDrivePWM() { + pinMode(2,OUTPUT); + pinMode(5,OUTPUT); + TCCR3B = _BV(WGM33)|_BV(WGM32)|_BV(CS30); + TCCR3A = _BV(COM3A1)|_BV(COM3B1)|_BV(WGM31);//WGM3_14; //COM3 and WGM3 = 14 _BV(COM3A0) + ICR3 = PWM_COUNT; // to count up to + OCR3A = PWM_COUNT/2; //default to 50% duty cycle + OCR3B = PWM_COUNT/2; //default to 50% duty cycle +} + +static void setDrivePWM(unsigned int duty_cycle) { + //Sets the new PWM duty cycle; duty cycle scaled from 0 to 65535 for 0 - 100 percent + + OCR3B = ((duty_cycle*(unsigned long)PWM_COUNT)/65535); +} + diff --git a/MainRobotCode/Conversion.h b/MainRobotCode/Conversion.h new file mode 100644 index 0000000..a01beb7 --- /dev/null +++ b/MainRobotCode/Conversion.h @@ -0,0 +1,15 @@ +#ifndef CONVERSION_H +#define CONVERSION_H +#include "Arduino.h" + +/* This has the conversion factors for various units. +*/ + +#define US_IN_SEC 1000000 //uSec per sec +static const float WHEEL_DIAMETER_INCH = 2.74; +static const long UM_PER_IN = 25400; +static const float WHEEL_DIAMETER_UM = WHEEL_DIAMETER_INCH*UM_PER_IN; +static const float TICKS_PER_REV = 8245.81; //on output shaft +static const long UM_PER_TICK = (WHEEL_DIAMETER_UM*PI)/TICKS_PER_REV; + +#endif diff --git a/MainRobotCode/DriveController.cpp b/MainRobotCode/DriveController.cpp new file mode 100644 index 0000000..e5e397e --- /dev/null +++ b/MainRobotCode/DriveController.cpp @@ -0,0 +1,104 @@ +#include "DriveController.h" +#include "Pinout.h" +#include "Rates.h" +#include "Conversion.h" +#include "DrivePWM.h" + +DriveController::DriveController() +{ + leftEnc = new Encoder(LEFT_ENC_REV?LEFT_ENC_CHAN_B:LEFT_ENC_CHAN_A,LEFT_ENC_REV?LEFT_ENC_CHAN_A:LEFT_ENC_CHAN_B); + rightEnc = new Encoder(RIGHT_ENC_REV?RIGHT_ENC_CHAN_B:RIGHT_ENC_CHAN_A,RIGHT_ENC_REV?RIGHT_ENC_CHAN_A:RIGHT_ENC_CHAN_B); + leftPID = new PID(100,0,0); + rightPID = new PID(100,0,0); +} + +long DriveController::getLeftEncCount() { + return leftEnc->read(); +} + +long DriveController::getRightEncCount() { + return rightEnc->read(); +} + +void DriveController::update() { //called each time in loop + static unsigned long long lastLoopTime = 0; + static long lastLeftPwr = 0; + static long lastRightPwr = 0; + static long lastLeftEnc = getLeftEncCount(); + static long lastRightEnc = getLeftEncCount(); + if((micros()-lastLoopTime)>MOTOR_CONTROLLER_PERIOD_US) + { + //Get time diff and enc diff + long long diffTime = micros()-lastLoopTime; + lastLoopTime = micros(); + long long leftDiffTicks = (leftEnc->read())-lastLeftEnc; + lastLeftEnc = leftEnc->read(); + long long rightDiffTicks = (rightEnc->read())-lastRightEnc; + lastRightEnc = rightEnc->read(); + //compute velocity + long long leftVel = ((leftDiffTicks*US_IN_SEC*UM_PER_TICK)/diffTime); //in mm/sec + long long rightVel = ((rightDiffTicks*US_IN_SEC*UM_PER_TICK)/diffTime); //in mm/sec + //compute PID + long long leftpidOut = leftPID -> compute(leftSetpoint,leftVel); + long long rightpidOut = rightPID -> compute(rightSetpoint,rightVel); + //set output + lastLeftPwr += leftpidOut; + lastLeftPwr = constrain(lastLeftPwr,-65535,65535); + lastRightPwr += rightpidOut; + lastRightPwr = constrain(lastRightPwr,-65535,65535); + if(leftSetpoint == 0) + lastLeftPwr = 0; + if(rightSetpoint == 0) + lastRightPwr = 0; + if(lastLeftPwr > 0) + { + setDrivePWM((unsigned int)lastLeftPwr, LEFT,FORWARD); + } + else + { + setDrivePWM((unsigned int)(-1*lastLeftPwr), LEFT,BACKWARD); + } + if(lastRightPwr > 0) + { + setDrivePWM((unsigned int)lastRightPwr, RIGHT,FORWARD); + } + else + { + setDrivePWM((unsigned int)(-1*lastRightPwr), RIGHT,BACKWARD); + } + } +} + +void DriveController::setup() { //called during setup + initDrivePWM();//sets up timer for PWM generation +} + +void DriveController::setLeftSetpoint(long newLeft) { + leftSetpoint = newLeft; +} + +void DriveController::setRightSetpoint(long newRight) { + rightSetpoint = newRight; +} + +void DriveController::setBothSetpoints(long newLeft,long newRight) { + setLeftSetpoint(newLeft); + setRightSetpoint(newRight); +} + +void DriveController::addToLeftSetpoint(long addLeft) { + leftSetpoint += addLeft; +} + +void DriveController::addToRightSetpoint(long addRight) { + rightSetpoint += addRight; +} + +void DriveController::addToBothSetpoints(long addLeft,long addRight) { + addToLeftSetpoint(addLeft); + addToRightSetpoint(addRight); +} + +void DriveController::stop() { + setBothSetpoints(0,0); +} diff --git a/MainRobotCode/DriveController.h b/MainRobotCode/DriveController.h new file mode 100644 index 0000000..f1ab1f6 --- /dev/null +++ b/MainRobotCode/DriveController.h @@ -0,0 +1,37 @@ +#ifndef DRIVE_CONTROLLER_H +#define DRIVE_CONTROLLER_H +#include "Arduino.h" +#include "Encoder.h" +#include "PID.h" +class DriveController { +public: + static DriveController* getInstance() { + static DriveController instance; + return &instance; + } + //Encoder access wrappers + long getLeftEncCount(); + long getRightEncCount(); + void update(); //called each time in loop + void setup(); //called during setup + void setLeftSetpoint(long newLeft); + void setRightSetpoint(long newRight); + void setBothSetpoints(long newLeft,long newRight); + void addToLeftSetpoint(long addLeft); + void addToRightSetpoint(long addRight); + void addToBothSetpoints(long addLeft,long addRight); + void stop(); //sets both setpoints to zero + static const long long DEFAULT_SETPOINT = 0.75*2.75*PI*2.54*10000; //0.75 RPS + static const long long MAX_SETPOINT = 0.9*2.75*PI*2.54*10000; //0.9 RPS + static const long long MIN_SETPOINT = 0.3*2.75*PI*2.54*10000; //0.3 RPS +private: + DriveController(); + Encoder* leftEnc; + Encoder* rightEnc; + PID* leftPID; + PID* rightPID; + long leftSetpoint; + long rightSetpoint; +}; + +#endif diff --git a/MainRobotCode/DrivePWM.h b/MainRobotCode/DrivePWM.h index a665518..c1ee27c 100644 --- a/MainRobotCode/DrivePWM.h +++ b/MainRobotCode/DrivePWM.h @@ -1,35 +1,90 @@ -#ifndef DRIVE_PWM -#define DRIVE_PWM +#ifndef DRIVE_PWM_H +#define DRIVE_PWM_H + #include +#include + typedef enum DriveSide { - LEFT = 5, //Pin number + LEFT = 5, //Pin numbers RIGHT = 2 }; -#define PWM_FREQ 16000 +typedef enum DriveDirection { + FORWARD = 1, + BACKWARD = 0 +}; + +#define LEFT_DIR_PIN 4 +#define LEFT_PWM_PIN 2 //can't change because implemented at timer register lvl +#define RIGHT_DIR_PIN 6 +#define RIGHT_PWM_PIN 5 //can't change because implemented at timer register lvl + +#define LEFT_REVERSED 1 +#define RIGHT_REVERSED 0 + +//PWM defines +#define PWM_FREQ 16000 //this isn't in rates because it isn't a control loop but the hardware timer PWM freq #define CLOCK_FREQ 16000000 -#define PWM_COUNT (((CLOCK_FREQ/PWM_FREQ))-1) +static const unsigned int PWM_COUNT = (((CLOCK_FREQ/PWM_FREQ))-1);// make constant to avoid recomputing each time static void initDrivePWM() { - pinMode(LEFT,OUTPUT); - pinMode(RIGHT,OUTPUT); + pinMode(LEFT_PWM_PIN,OUTPUT); + pinMode(RIGHT_PWM_PIN,OUTPUT); + pinModeFast(LEFT_DIR_PIN,OUTPUT); + pinModeFast(RIGHT_DIR_PIN,OUTPUT); TCCR3B = _BV(WGM33)|_BV(WGM32)|_BV(CS30); TCCR3A = _BV(COM3A1)|_BV(COM3B1)|_BV(WGM31);//WGM3_14; //COM3 and WGM3 = 14 _BV(COM3A0) ICR3 = PWM_COUNT; // to count up to - OCR3A = PWM_COUNT/2; //default to 50% duty cycle - OCR3B = PWM_COUNT/2; //default to 50% duty cycle + OCR3A = 0; //default to off + OCR3B = 0; //default to off + } -static void setDrivePWM(unsigned int duty_cycle, DriveSide side) { +static void setDrivePWM(unsigned int duty_cycle, DriveSide side,DriveDirection newDir) { //Sets the new PWM duty cycle; duty cycle scaled from 0 to 65535 for 0 - 100 percent + constrain(duty_cycle,0,65535); if(side == LEFT) { - OCR3A = ((duty_cycle*PWM_COUNT)/65535); + OCR3A = ((duty_cycle*(unsigned long)PWM_COUNT)/65535); + if(newDir == FORWARD) + { + if(LEFT_REVERSED) + digitalWriteFast(LEFT_DIR_PIN,BACKWARD); + else + digitalWriteFast(LEFT_DIR_PIN,FORWARD); + } + else if(newDir == BACKWARD) + { + if(LEFT_REVERSED) + digitalWriteFast(LEFT_DIR_PIN,FORWARD); + else + digitalWriteFast(LEFT_DIR_PIN,BACKWARD); + } } else if (side == RIGHT) { - OCR3B = ((duty_cycle*PWM_COUNT)/65535); + OCR3B = ((duty_cycle*(unsigned long)PWM_COUNT)/65535); + if(newDir == FORWARD) + { + if(RIGHT_REVERSED) + digitalWriteFast(RIGHT_DIR_PIN,BACKWARD); + else + digitalWriteFast(RIGHT_DIR_PIN,FORWARD); + } + else if(newDir == BACKWARD) + { + if(RIGHT_REVERSED) + digitalWriteFast(RIGHT_DIR_PIN,FORWARD); + else + digitalWriteFast(RIGHT_DIR_PIN,BACKWARD); + } } } +static void stopDrive() { + setDrivePWM(0, LEFT,FORWARD); + setDrivePWM(0, RIGHT,FORWARD); +} + #endif + diff --git a/MainRobotCode/ForwardState.cpp b/MainRobotCode/ForwardState.cpp new file mode 100644 index 0000000..9b83bda --- /dev/null +++ b/MainRobotCode/ForwardState.cpp @@ -0,0 +1,52 @@ +#include "ForwardState.h" +#include "GlobalInstances.h" +#include "RobotOdometry.h" + +ForwardState::ForwardState() { + started = false; + startX = 0; //mm + startY = 0; //mm + stopTime = 0; + stopping = false; + forwardTarget = 0;//mm dist +} + +void ForwardState::handle() { + if(started == false) + { + startX = getXLoc(); + startY = getYLoc(); + started = true; + } + long diffX = abs(getXLoc()-startX); + long diffY = abs(getYLoc()-startY); + long distTravel = sqrt((diffX*diffX)+(diffY*diffY)); + if((forwardTarget-distTravel)>FORWARD_TOL) + { + myDriveControl->setBothSetpoints(DriveController::DEFAULT_SETPOINT,DriveController::DEFAULT_SETPOINT); + } + else if((distTravel-forwardTarget)>FORWARD_TOL) + { + myDriveControl->setBothSetpoints(-1*DriveController::DEFAULT_SETPOINT,-1*DriveController::DEFAULT_SETPOINT); + } + else + { + //in tol + myDriveControl->stop(); + if(stopping == false) + { + stopping = true; + stopTime = micros(); + } + if((micros()-stopTime)>500000) + { + stopping = false; + started = false; + currentState = previousState; + previousState = FORWARD_DIST; + } + } +} +void ForwardState::setForwardTarget(long long newTarget) { + forwardTarget = newTarget; +} diff --git a/MainRobotCode/ForwardState.h b/MainRobotCode/ForwardState.h new file mode 100644 index 0000000..c71ff61 --- /dev/null +++ b/MainRobotCode/ForwardState.h @@ -0,0 +1,26 @@ +#ifndef FORWARD_STATE_H +#define FORWARD_STATE_H +#include "Arduino.h" +#include "DriveController.h" +class ForwardState { +public: +static ForwardState* getInstance() { + static ForwardState instance; + return &instance; +} +void handle(); +void setForwardTarget(long long newTarget); +private: +ForwardState(); +bool started; +long startX; //mm +long startY; //mm +long long stopTime; +bool stopping; +long long forwardTarget;//mm dist + +//Constants +static const int FORWARD_TOL = 0.5*25.4;//mm +}; + +#endif diff --git a/MainRobotCode/GlobalInstances.h b/MainRobotCode/GlobalInstances.h new file mode 100644 index 0000000..49ea36e --- /dev/null +++ b/MainRobotCode/GlobalInstances.h @@ -0,0 +1,36 @@ +#ifndef GLOBAL_INSTANCES_H +#define GLOBAL_INSTANCES_H + +#include "DriveController.h" +#include "WallFollowState.h" +#include "ForwardState.h" +#include "TurnState.h" +#include "SpecCaseState.h" + +static DriveController* myDriveControl; +static WallFollowState* myWallState; +static ForwardState* myForwardState; +static TurnState* myTurnState; +static SpecCaseState* mySpecCaseState; + +static void initalizeInstances() { + myDriveControl = DriveController::getInstance(); + myDriveControl->setup(); + myWallState = WallFollowState::getInstance(); + myForwardState = ForwardState::getInstance(); + myTurnState = TurnState::getInstance(); + mySpecCaseState = SpecCaseState::getInstance(); +} + +typedef enum RobotState { + WALL_FOLLOW, + TURN, + STANDBY, + SPEC_CASE, //Detected that front wall sensor maxed and other isn't + FORWARD_DIST +}; + +static RobotState currentState = STANDBY; +static RobotState previousState = STANDBY; + +#endif diff --git a/MainRobotCode/MainRobotCode.ino b/MainRobotCode/MainRobotCode.ino index cfaf532..1fa3516 100644 --- a/MainRobotCode/MainRobotCode.ino +++ b/MainRobotCode/MainRobotCode.ino @@ -1,10 +1,50 @@ +#define DEBUG_PRINT //used to enable debug print statements +#include #include "DrivePWM.h" +#include "WallMath.h" +#include "PID.h" +#include "debug.h" +#include "Encoder.h" +#include "Rates.h" +#include "RobotOdometry.h" +#include "DriveController.h" +#include "GlobalInstances.h" + void setup() { - initDrivePWM(); + DebugBegin(); + DebugPrintln("Serial started"); + wallInit(); + initalizeInstances(); } void loop() { - setDrivePWM(65535*0.75,LEFT); - setDrivePWM(65535*0.5,RIGHT); - delay(500); + switch(currentState) + { + case STANDBY://add btn later + delay(1000); + currentState = WALL_FOLLOW; + break; + case SPEC_CASE: + mySpecCaseState->handle(); + break; + case WALL_FOLLOW: + myWallState -> handle(); + break; + case TURN: + myTurnState->handle(); + break; + case FORWARD_DIST: + myForwardState->handle(); + break; + default: + break; + } + myDriveControl->update(); + computeOdometry(myDriveControl); +/* DebugPrint(getXLoc()); //mm + DebugPrint('\t'); + DebugPrint(getYLoc()); //mm + DebugPrint('\t'); + DebugPrintln(getTheta()*(180/PI)); //deg*/ } + diff --git a/MainRobotCode/PID.cpp b/MainRobotCode/PID.cpp new file mode 100644 index 0000000..6aa93ba --- /dev/null +++ b/MainRobotCode/PID.cpp @@ -0,0 +1,25 @@ +#include "PID.h" + +PID::PID(long long p,long long i,long long d) { + kp = p; + ki = i; + kd = d; + lastTime = micros(); + lastError = 0; + intError = 0; +} +long long PID::compute(long long set,long long act) {//recalculates output term + long long diffTime = micros()-lastTime; + lastTime = micros(); + long long error = set-act; + long long diffError = ((error-lastError)*1000000)/diffTime; + lastError = error; + intError += (error*diffTime)/1000000; + #define K_GAIN 10000 + return ((error*kp)/K_GAIN)+((intError*ki)/K_GAIN)+((diffError*kd)/K_GAIN); +} +void PID::reset() { + intError = 0; + lastError = 0; +} + diff --git a/MainRobotCode/PID.h b/MainRobotCode/PID.h new file mode 100644 index 0000000..a14c61e --- /dev/null +++ b/MainRobotCode/PID.h @@ -0,0 +1,18 @@ +#ifndef PID_h +#define PID_h +#include "Arduino.h" +class PID { +public: +PID(long long p,long long i,long long d); +long long compute(long long set,long long act);//returns output term +void reset(); +private: +long long kp; +long long ki; +long long kd; +long long lastTime; +long long intError; //integral of error; in error*microseconds +long long lastError; +}; +#endif + diff --git a/MainRobotCode/Pinout.h b/MainRobotCode/Pinout.h new file mode 100644 index 0000000..1b257d0 --- /dev/null +++ b/MainRobotCode/Pinout.h @@ -0,0 +1,12 @@ +#ifndef PINOUT_H +#define PINOUT_H + +#define LEFT_ENC_CHAN_A 20 +#define LEFT_ENC_CHAN_B 19 +#define LEFT_ENC_REV 0 + +#define RIGHT_ENC_CHAN_A 18 +#define RIGHT_ENC_CHAN_B 3 +#define RIGHT_ENC_REV 0 + +#endif diff --git a/MainRobotCode/Rates.h b/MainRobotCode/Rates.h new file mode 100644 index 0000000..6a85143 --- /dev/null +++ b/MainRobotCode/Rates.h @@ -0,0 +1,19 @@ +#ifndef RATES_H +#define RATES_H + +/* This file has all the timing for the various control loop rates. + Try not to change these unless you know what your doing because + it might change system stability for example if you increase + the motor velocity PID then we might not have many ticks to + base speed off of.*/ + +#define ODOMETRY_FREQ 200 //hz +#define ODOMETRY_PERIOD_US 1000000/ODOMETRY_FREQ //cycle period in usec + +#define MOTOR_CONTROLLER_FREQ 200 //hz +#define MOTOR_CONTROLLER_PERIOD_US 1000000/MOTOR_CONTROLLER_FREQ //cycle period in usec + +#define WALL_FOLLOW_FREQ 60 //hz +#define WALL_FOLLOW_PERIOD_US 1000000/WALL_FOLLOW_FREQ //cycle period in usec + +#endif diff --git a/MainRobotCode/RobotOdometry.cpp b/MainRobotCode/RobotOdometry.cpp new file mode 100644 index 0000000..187e27a --- /dev/null +++ b/MainRobotCode/RobotOdometry.cpp @@ -0,0 +1,38 @@ +#include "RobotOdometry.h" +#include "Arduino.h" + +void computeOdometry(DriveController* myController) { //called as fast as possible, it handles timing + if((micros()-myPosition.lastOdometryTime) > ODOMETRY_PERIOD_US) //controls update rate + { + myPosition.lastOdometryTime = micros(); + long leftReading = myController->getLeftEncCount(); + long rightReading = myController->getRightEncCount(); + long leftDiff = leftReading-myPosition.lastLeftEnc; + long rightDiff = rightReading-myPosition.lastRightEnc; + myPosition.lastLeftEnc = leftReading; + myPosition.lastRightEnc = rightReading; + float leftDist = leftDiff*LEFT_DIST_PER_TICK; + float rightDist = rightDiff*RIGHT_DIST_PER_TICK; + + float distTraveled = (leftDist+rightDist)/2.0; + myPosition.x_loc += sin((PI/2)-(myPosition.theta/1000.0))*distTraveled; + myPosition.y_loc += -1*cos((PI/2)-(myPosition.theta/1000.0))*distTraveled; + + float encDeltaTheta = ((rightDist-leftDist)/WHEEL_BASE)*1000; //mRad + //Serial.println(myPosition.theta); + myPosition.theta += encDeltaTheta; //add gyrodometry here + if(myPosition.theta > (2*PI*1000)) + myPosition.theta = myPosition.theta-(2*PI*1000); + + } +} + +long getXLoc() {//Returns value in mm + return (myPosition.x_loc/1000.0f); +} +long getYLoc() {//Returns value in mm + return (myPosition.y_loc/1000.0f); +} +float getTheta() {//Returns value in Rad + return (myPosition.theta/1000.0f); +} diff --git a/MainRobotCode/RobotOdometry.h b/MainRobotCode/RobotOdometry.h new file mode 100644 index 0000000..baa8808 --- /dev/null +++ b/MainRobotCode/RobotOdometry.h @@ -0,0 +1,31 @@ +#ifndef ROBOT_ODOMETRY_H +#define ROBOT_ODOMETRY_H +#include "Arduino.h" +#include "Encoder.h" +#include "Rates.h" +#include "DriveController.h" +struct RobotPos { + long x_loc = 0; //um + long y_loc = 0; //um + float theta = 0; //mRad + long lastLeftEnc = 0; + long lastRightEnc = 0; + long long lastOdometryTime = 0; //us +}; +static RobotPos myPosition; + +void computeOdometry(DriveController* myController); //called as fast as possible, it handles timing + +long getXLoc();//Returns value in mm +long getYLoc();//Returns value in mm +float getTheta();//Returns value in Rad + +static const float TICKS_PER_REV = 8245.81; +static const float LEFT_DIST_PER_REV = 2.74*25.4*1000*PI; +static const float RIGHT_DIST_PER_REV = 2.74*25.4*1000*PI; +static const float LEFT_DIST_PER_TICK = LEFT_DIST_PER_REV/TICKS_PER_REV; +static const float RIGHT_DIST_PER_TICK = RIGHT_DIST_PER_REV/TICKS_PER_REV; + +static const float WHEEL_BASE = 10*25.4*1000; //um check if 10 in is correct correct + +#endif diff --git a/MainRobotCode/SpecCaseState.cpp b/MainRobotCode/SpecCaseState.cpp new file mode 100644 index 0000000..8ff37f7 --- /dev/null +++ b/MainRobotCode/SpecCaseState.cpp @@ -0,0 +1,62 @@ +#include "SpecCaseState.h" +#include "GlobalInstances.h" +#include "RobotOdometry.h" + +SpecCaseState::SpecCaseState() { +} + +void SpecCaseState::handle() { + static enum SpecCaseStep{ + FORWARD_FIRST, + TURN_FIRST, + FORWARD_SECOND, + TURN_SECOND, + FORWARD_THIRD, + DONE + } myState = FORWARD_FIRST; + switch(myState) + { + case FORWARD_FIRST: + previousState = currentState; + //set forward target + myForwardState->setForwardTarget(25.4*10); + currentState = FORWARD_DIST; + myState = TURN_FIRST; + break; + case TURN_FIRST: + previousState = currentState; + //set turn target + myTurnState->setTurnHeading(getTheta()-(PI/2)); + currentState = TURN; + myState = FORWARD_SECOND; + break; + case FORWARD_SECOND: + previousState = currentState; + //set forward target + myForwardState->setForwardTarget(25.4*22); + currentState = FORWARD_DIST; + myState = TURN_SECOND; + break; + case TURN_SECOND: + previousState = currentState; + //set turn target + myTurnState->setTurnHeading(getTheta()-(PI/2)); + currentState = TURN; + myState = FORWARD_THIRD; + break; + case FORWARD_THIRD: + previousState = currentState; + //set forward target + myForwardState->setForwardTarget(25.4*15); + currentState = FORWARD_DIST; + myState = DONE; + break; + case DONE: + previousState = currentState; + currentState = WALL_FOLLOW; + myState = FORWARD_FIRST; + break; + default: + break; + } +} diff --git a/MainRobotCode/SpecCaseState.h b/MainRobotCode/SpecCaseState.h new file mode 100644 index 0000000..eda6e37 --- /dev/null +++ b/MainRobotCode/SpecCaseState.h @@ -0,0 +1,18 @@ +#ifndef SPEC_CASE_STATE_H +#define SPEC_CASE_STATE_H + +#include "Arduino.h" + +class SpecCaseState { +public: + static SpecCaseState* getInstance() { + static SpecCaseState instance; + return &instance; + } + void handle(); +private: + SpecCaseState(); + +}; + +#endif diff --git a/MainRobotCode/TurnState.cpp b/MainRobotCode/TurnState.cpp new file mode 100644 index 0000000..099bb29 --- /dev/null +++ b/MainRobotCode/TurnState.cpp @@ -0,0 +1,52 @@ +#include "TurnState.h" +#include "RobotOdometry.h" +#include "GlobalInstances.h" +TurnState::TurnState() { + targetTurnHeading=0; + turnPid = new PID(1250000,0,0); +} + +void TurnState::handle() { + static long long stoppedTime = 0; + static bool doneTurn = false; + //Serial.println("Turn"); + if(targetTurnHeading>(2*PI)) + { + targetTurnHeading -= (2*PI); + } + float currentHeading = getTheta(); + float absError = abs(currentHeading-targetTurnHeading); + if(absError>PI) absError = (2*PI)-absError; + long long pidOut = abs(turnPid -> compute(0,abs(currentHeading)*10000)); //scaled because pid uses long longs + if((targetTurnHeading>currentHeading) xor (abs(targetTurnHeading-currentHeading)>PI)) + { + //turn counter clockwise + myDriveControl->setLeftSetpoint(-1*pidOut); + myDriveControl->setRightSetpoint(pidOut); + } + else + { + //turn clockwise + myDriveControl->setLeftSetpoint(pidOut); + myDriveControl->setRightSetpoint(-1*pidOut); + } + if(abs(getTheta()-targetTurnHeading)stop(); //stop motors + if((micros()-stoppedTime)>500000) + { + currentState = previousState; + previousState = TURN; + doneTurn = false; + } + } +} + +void TurnState::setTurnHeading(float newHeading) { + targetTurnHeading = newHeading; +} diff --git a/MainRobotCode/TurnState.h b/MainRobotCode/TurnState.h new file mode 100644 index 0000000..b035610 --- /dev/null +++ b/MainRobotCode/TurnState.h @@ -0,0 +1,21 @@ +#ifndef TURN_STATE_H +#define TURN_STATE_H +#include "Arduino.h" +#include "PID.h" +class TurnState { +public: +static TurnState* getInstance() { + static TurnState instance; + return &instance; +} +void handle(); +void setTurnHeading(float newHeading); +private: +TurnState(); +#define TURN_TOL 1*(PI/180.0) //Rad +float targetTurnHeading; //Rad - based on field abs heading +PID* turnPid; + +}; +#endif + diff --git a/MainRobotCode/WallFollowState.cpp b/MainRobotCode/WallFollowState.cpp new file mode 100644 index 0000000..fe1981e --- /dev/null +++ b/MainRobotCode/WallFollowState.cpp @@ -0,0 +1,55 @@ +#include "WallFollowState.h" +#include "WallMath.h" +#include "RobotOdometry.h" +#include "Rates.h" +#include "GlobalInstances.h" +#include "debug.h" + +WallFollowState::WallFollowState() { + stoppedTime = 0; + startTurn = false; + lastLoopTime = 0; + walloffsetpidOut = 0; + wallthetapidOut = 0; + wallOffsetPid = new PID(3000000,0,0);//600000 + wallThetaPid = new PID(600000,0,0); +} + +void WallFollowState::handle() { + if((micros()-lastLoopTime) >WALL_FOLLOW_PERIOD_US) + { + lastLoopTime = micros(); + WallState newState = getWallState(RIGHT_WALL); + //DebugPrintln((newState.frontDist < (25.4*10))&&(newState.frontDist>5)); + if(newState.frontDist < (25.4*6)) + { + myDriveControl->stop(); //stop if wall in front is too close + if(startTurn == false) + { + startTurn = true; + stoppedTime = micros(); + } + myTurnState->setTurnHeading(getTheta()+(PI/2.0)); + if((micros()-stoppedTime)>100000) + { + startTurn = false; + currentState = TURN; + previousState = WALL_FOLLOW; + DebugPrintln("Back to turn"); + } + } + else if(newState.specCase) + { + currentState = SPEC_CASE; + } + else + { + walloffsetpidOut = wallOffsetPid -> compute(WALL_OFFSET_SETPOINT,newState.wallDist); + wallthetapidOut = wallThetaPid -> compute(0,newState.theta); + long newLeft = DriveController::DEFAULT_SETPOINT+wallthetapidOut-walloffsetpidOut; + long newRight = DriveController::DEFAULT_SETPOINT+walloffsetpidOut-wallthetapidOut; + myDriveControl->setLeftSetpoint(abs(newLeft)); + myDriveControl->setRightSetpoint(abs(newRight)); + } + } +} diff --git a/MainRobotCode/WallFollowState.h b/MainRobotCode/WallFollowState.h new file mode 100644 index 0000000..64640c1 --- /dev/null +++ b/MainRobotCode/WallFollowState.h @@ -0,0 +1,24 @@ +#ifndef WALL_FOLLOW_STATE_H +#define WALL_FOLLOW_STATE_H +#include "Arduino.h" +#include "DriveController.h" +class WallFollowState { +public: + static WallFollowState* getInstance() { + static WallFollowState instance; + return &instance; + } + void handle(); //called from main code +private: + WallFollowState(); + long long lastLoopTime; + long long stoppedTime; + bool startTurn; + long long walloffsetpidOut = 0; + long long wallthetapidOut = 0; + PID* wallOffsetPid; + PID* wallThetaPid; + + static const long long WALL_OFFSET_SETPOINT = (long)10*25.4; +}; +#endif diff --git a/MainRobotCode/WallMath.h b/MainRobotCode/WallMath.h new file mode 100644 index 0000000..679b3e3 --- /dev/null +++ b/MainRobotCode/WallMath.h @@ -0,0 +1,87 @@ +#ifndef WALL_MATH_H +#define WALL_MATH_H + +#include + +//(a/(1000*(x+b)))-42 //return dist in mm +#define IR_ZERO_DIST(x) (((48955435/(x+33))-4200)/1000) //return dist in mm +#define IR_ONE_DIST(x) (((49281348/(x+9))-4200)/1000) //return dist in mm +#define IR_TWO_DIST(x) (((50133745/(x+20))-4200)/1000) //return dist in mm +#define IR_THREE_DIST(x) ((x>70)?(((81641309/(x-65))-4200)/1000):(60*25)) //return dist in mm +#define IR_FOUR_DIST(x) ((x>70)?(((44177767/(x-66))-4200)/1000):(60*25)) //return dist in mm + +static void wallInit() { + analogReference(INTERNAL2V56); //Better range for IR sensors which max out at 2.25v + ADCSRA &= ~0x7; // remove clk prescaler arduino set + ADCSRA |= (1<(25.4*7))&&(d2<(7*25.4))) + { + newState.specCase = true; + } + else + { + newState.specCase = false; + } + + long k1 = sqrt((Y_SENS*Y_SENS)+((d2-d1)*(d2-d1))); + newState.wallDist = calculateXOffset(d1,d2,k1); + newState.theta = ((PI/2)-acos((d2-d1)/(float)Y_SENS)); + if(d3 > MAX_IR_DIST) + { + newState.frontDist = -1; + return newState; + } + else + { + newState.frontDist = calculateFrontDist(d3, k1); + } + return newState; +} + + +#endif diff --git a/MainRobotCode/debug.h b/MainRobotCode/debug.h new file mode 100644 index 0000000..243828e --- /dev/null +++ b/MainRobotCode/debug.h @@ -0,0 +1,16 @@ +#ifndef DEBUG_H +#define DEBUG_H + +#include + +#if defined(DEBUG_PRINT) + #define DebugBegin() Serial.begin(115200) + #define DebugPrintln(a) Serial.println(a) + #define DebugPrint(a) Serial.print(a) +#else + #define DebugBegin() + #define DebugPrintln(a) + #define DebugPrint(a) +#endif + +#endif diff --git a/MotorPIDTest/MotorPIDTest.ino b/MotorPIDTest/MotorPIDTest.ino new file mode 100644 index 0000000..3d08e2f --- /dev/null +++ b/MotorPIDTest/MotorPIDTest.ino @@ -0,0 +1,140 @@ +#include "digitalIOPerformance.h" +#include "PreciseTime.h" +#include "PID.h" +volatile unsigned long count; +byte previousState; +unsigned long long lastTime; + +const float setRPS = 0.5; +const float ticksPerSecond = 8245.81/setRPS; +const float usPerTick = 1000000/ticksPerSecond; +const unsigned long TEST_SETPOINT = (unsigned long)usPerTick*16; + +#define PWM_FREQ 16000 +#define CLOCK_FREQ 16000000 +static const unsigned int PWM_COUNT = (((CLOCK_FREQ/PWM_FREQ))-1);// make constant to avoid recomputing each time + +const unsigned long kp = 1; +const unsigned long ki = 0; +const unsigned long kd = 0; +unsigned long pidIn; +unsigned long pidOut; +unsigned long setPoint = 0; +PID myPid(kp,ki,kd,&pidIn,&pidOut,setPoint); +void setup() { + // put your setup code here, to run once: + pinModeFast(18,INPUT); + pinModeFast(19,INPUT); + count = 0; + byte chanA = digitalReadFast(18); + byte chanB = digitalReadFast(19); + previousState = getState(chanA,chanB); + initDrivePWM(); + pinMode(4,OUTPUT); + digitalWrite(4,HIGH); + Serial.begin(115200); + interrupts(); + attachInterrupt(digitalPinToInterrupt(18), leftEncInterrupt, CHANGE); + attachInterrupt(digitalPinToInterrupt(19), leftEncInterrupt, CHANGE); + initPreciseGlobal(); + lastTime = preciseTime(); + myPid.setSetpoint(TEST_SETPOINT); +} +unsigned long lastCount = 0; +unsigned long long lastPTime = 0; +void loop() { + lastPTime = preciseTime(); + delay(1000); + lastPTime = preciseTime()-lastPTime; + unsigned long diffCount = count-lastCount; + float RPS = (diffCount/8245.81)/(lastPTime/16000000.0); + unsigned long tDiff = (unsigned long)lastPTime/16; + noInterrupts(); + unsigned long pIn = (unsigned long)pidIn; + unsigned long pOut = (unsigned long)pidOut; + interrupts(); + /* Serial.print(tDiff); + Serial.print('\t'); + Serial.print(diffCount); + Serial.print('\t'); + Serial.print(pIn); + Serial.print('\t'); + Serial.print(pOut); + Serial.print('\t'); + Serial.println(RPS);*/ + unsigned long a = micros(); + myPid.compute(); + a = micros()-a; + Serial.println(a); +// Serial.println((unsigned long)TEST_SETPOINT); + lastCount = count; + + +} +byte getState(byte chanA_val,byte chanB_val) { + if(~chanA_val&chanB_val) + return 0x00; + else if(chanA_val&chanB_val) + return 0x01; + else if(chanA_val&(~chanB_val)) + return 0x02; + else + return 0x03; +} + +static void initDrivePWM() { + pinMode(2,OUTPUT); + pinMode(5,OUTPUT); + TCCR3B = _BV(WGM33)|_BV(WGM32)|_BV(CS30); + TCCR3A = _BV(COM3A1)|_BV(COM3B1)|_BV(WGM31);//WGM3_14; //COM3 and WGM3 = 14 _BV(COM3A0) + ICR3 = PWM_COUNT; // to count up to + OCR3A = PWM_COUNT/2; //default to 50% duty cycle + OCR3B = PWM_COUNT/2; //default to 50% duty cycle +} + +static void setDrivePWM(unsigned int duty_cycle) { + //Sets the new PWM duty cycle; duty cycle scaled from 0 to 65535 for 0 - 100 percent + constrain(duty_cycle,0,65535); + OCR3B = ((duty_cycle*(unsigned long)PWM_COUNT)/65535); +} + +void leftEncInterrupt() { + //get diff time + unsigned long long newTime = preciseTime(); + unsigned long long diffTime = newTime-lastTime; + lastTime = preciseTime(); + pidIn = diffTime; + //compute enc count + byte chanA = digitalReadFast(18); + byte chanB = digitalReadFast(19); + byte state = getState(chanA,chanB); + //byte state = ((~chanB)<<1)|((chanA&chanB)|(~(chanA|chanB))); //0-3 + if((previousState == 0x03) && (state==0x00)) + { + //wrapped around + count++; + } + else if((previousState == 0x00) && (state==0x03)) + { + //wrapped around + count--; + } + else if(state>previousState) + { + count++; + } + else if(state65535) + newVal = 65535; + else if(newVal<0) + newVal = 0;*/ + setDrivePWM(65535);//(newVal); +} diff --git a/MotorPIDTest/PID.cpp b/MotorPIDTest/PID.cpp new file mode 100644 index 0000000..f6b1632 --- /dev/null +++ b/MotorPIDTest/PID.cpp @@ -0,0 +1,43 @@ +#include "PID.h" +#include "PreciseTime.h" +PID::PID(unsigned long kp_new,unsigned long ki_new,unsigned long kd_new,unsigned long* input_new,unsigned long* output_new,unsigned long setpoint_new) { +i_sum = 0; +kp = kp_new; +ki = ki_new; +kd = kd_new; +input = input_new; +output = output_new; +setpoint = setpoint_new; +lastError = 0; +lastTime = preciseTime(); //in 1/16us ticks +} +PID::~PID() { +} +void PID::compute() { + //get time diff + static unsigned long long newTime; + newTime = preciseTime(); + static unsigned long long diff; + diff = (unsigned long)(newTime - lastTime); + //get error + static unsigned long error; + error = *input-setpoint; + //compute kp + static unsigned long newOutput; + newOutput = error*kp; + //compute ki + i_sum += (error*diff); + newOutput += (i_sum*ki) ; + //compute kd + newOutput += kd*(error - lastError)/diff; + *output = newOutput; +} +void PID::setSetpoint(unsigned long setpoint_new){ // also calls reset + setpoint = setpoint_new; + reset(); +} +void PID::reset() { //clears i term + i_sum = 0; + lastError = 0; + lastTime = preciseTime(); +} diff --git a/MotorPIDTest/PID.h b/MotorPIDTest/PID.h new file mode 100644 index 0000000..b1fd179 --- /dev/null +++ b/MotorPIDTest/PID.h @@ -0,0 +1,23 @@ +#ifndef PID_H +#define PID_H + +class PID { +public: +// Constructor/destructor +PID(unsigned long kp_new,unsigned long ki_new,unsigned long kd_new,unsigned long * input_new,unsigned long * output_new,unsigned long setpoint_new) ; +~PID() ; +void compute(); +void setSetpoint(unsigned long setpoint_new); // also calls reset +void reset(); //clears i term +private: +unsigned long i_sum; +unsigned long kp; +unsigned long ki; +unsigned long kd; +unsigned long* input; +unsigned long* output; +unsigned long setpoint; +unsigned long long lastTime; +unsigned long lastError; +}; +#endif diff --git a/MotorPIDTest/PreciseTime.cpp b/MotorPIDTest/PreciseTime.cpp new file mode 100644 index 0000000..1f85279 --- /dev/null +++ b/MotorPIDTest/PreciseTime.cpp @@ -0,0 +1,21 @@ +#include "PreciseTime.h" +#include "Arduino.h" + +unsigned int precise_overflow = 0; + +void initPreciseGlobal() { //sets up timer 5 for counting + TCCR5A = 0x00; // Normal mode; No output comparitors used + TCCR5B = 0x01; //No clk prescaler + TIMSK5 = 0x01; + interrupts(); +} +unsigned long long preciseTime() { //time in 1/16 us ticks + //reads timer and overflows, updates precise + return (((precise_overflow*(unsigned long long)65536)+(unsigned long long)TCNT5)); +} + +ISR(TIMER5_OVF_vect) +{ + precise_overflow++; +} + diff --git a/MotorPIDTest/PreciseTime.h b/MotorPIDTest/PreciseTime.h new file mode 100644 index 0000000..7fe3797 --- /dev/null +++ b/MotorPIDTest/PreciseTime.h @@ -0,0 +1,7 @@ +#ifndef PRECISE_TIME_H +#define PRECISE_TIME_H + +void initPreciseGlobal(); +unsigned long long preciseTime(); + +#endif diff --git a/MotorTestPlot.ods b/MotorTestPlot.ods new file mode 100644 index 0000000..a5daaf3 Binary files /dev/null and b/MotorTestPlot.ods differ diff --git a/MotorTorquePlots.ods b/MotorTorquePlots.ods new file mode 100644 index 0000000..88468c5 Binary files /dev/null and b/MotorTorquePlots.ods differ diff --git a/PWM_Test/PWM_Test.ino b/PWM_Test/PWM_Test.ino new file mode 100644 index 0000000..340805c --- /dev/null +++ b/PWM_Test/PWM_Test.ino @@ -0,0 +1,45 @@ +void setup() { + // put your setup code here, to run once: + initDrivePWM(); + pinMode(4,OUTPUT); + digitalWrite(4,HIGH); + pinMode(6,OUTPUT); + digitalWrite(6,HIGH); + Serial.begin(115200); + //setDrivePWM(65000); +} + +#define PWM_FREQ 16000 +#define CLOCK_FREQ 16000000 +static const unsigned int PWM_COUNT = (((CLOCK_FREQ/PWM_FREQ))-1);// make constant to avoid recomputing each time + +void loop() { + // put your main code here, to run repeatedly: + + for(unsigned int i = 20000;i<65535;i+=5000) + { + setDrivePWM(i); + delay(1000); + //delay(1000); + } + +} + +static void initDrivePWM() { + pinMode(2,OUTPUT); + pinMode(5,OUTPUT); + TCCR3B = _BV(WGM33)|_BV(WGM32)|_BV(CS30); + TCCR3A = _BV(COM3A1)|_BV(COM3B1)|_BV(WGM31);//WGM3_14; //COM3 and WGM3 = 14 _BV(COM3A0) + ICR3 = PWM_COUNT; // to count up to + OCR3A = PWM_COUNT/2;///2; //default to 50% duty cycle + OCR3B = PWM_COUNT/2;///2; //default to 50% duty cycle +} + +static void setDrivePWM(unsigned long duty_cycle) { + //Sets the new PWM duty cycle; duty cycle scaled from 0 to 65535 for 0 - 100 percent + + OCR3B = ((duty_cycle*(unsigned long)PWM_COUNT)/65535); + OCR3A = ((duty_cycle*(unsigned long)PWM_COUNT)/65535); + Serial.println(((duty_cycle*(unsigned long)PWM_COUNT)/65535)); +} + diff --git a/PrecisionTimerTesting/PrecisionTimerTesting.ino b/PrecisionTimerTesting/PrecisionTimerTesting.ino new file mode 100644 index 0000000..c4a34be --- /dev/null +++ b/PrecisionTimerTesting/PrecisionTimerTesting.ino @@ -0,0 +1,42 @@ +void setup() { + // put your setup code here, to run once: + Serial.begin(115200); + initPreciseGlobal(); +} +unsigned int precise_overflow = 0; +void loop() { + unsigned long long orig = preciseTime(); + unsigned long m_orig = millis(); + while((millis()-m_orig) < 10){} + orig = preciseTime() - orig; + //unsigned long diff = orig; +// noInterrupts(); + uint64_t pipe = preciseTime();//lets assume the data is 12345ABCD9 hexadecimal + char buf[50]; + if(pipe > 0xFFFFFFFFLL) { + sprintf(buf, "%lu%08lu", (unsigned long)(pipe>>32), (unsigned long)(pipe&0xFFFFFFFFULL)); + } else { + sprintf(buf, "%lu", (unsigned long)pipe); + } + //Serial.println( buf ); + Serial.println(buf); +} + + + +static void initPreciseGlobal() { //sets up timer 5 for counting + TCCR5A = 0x00; // Normal mode; No output comparitors used + TCCR5B = 0x01; //No clk prescaler + TIMSK5 = 0x01; + interrupts(); +} +static unsigned long long preciseTime() { + //reads timer and overflows, updates precise + return ((precise_overflow*(unsigned long long)65536)+(unsigned long long)TCNT5); +} + +ISR(TIMER5_OVF_vect) +{ + precise_overflow++; +// TIFR5 = 0x00;//clear flag +} diff --git a/SimplifiedWallMath.m b/SimplifiedWallMath.m new file mode 100644 index 0000000..6392956 --- /dev/null +++ b/SimplifiedWallMath.m @@ -0,0 +1,39 @@ +clear all; +%close all; +numOffsetSamples = 100; +for i = 1:numOffsetSamples +%% setup sensor accuracy +a = 4417.7766959627; +b = -66.1650874647; +std = 69.03631579; +%% setup robot params +xs = 4.5; %inch +ys = 3; +%% setup sensor readings +d_1 = 3.5;%3.5; %inch +d_2 = 3.5;%3.5; %inch +numSamplesToAvg = 16;%4^2; +d_1_total = 0; +d_2_total = 0; +for j=1:numSamplesToAvg + %%compute Sensor Error - using avg + val_1 = ((1/((d_1*2.54)+.42))*a) - b; + val_1 = normrnd(val_1,std,1,1); + val_2 = ((1/((d_2*2.54)+.42))*a) - b; + val_2 = normrnd(val_2,std,1,1); + d_1_total = d_1_total+val_1; + d_2_total = d_2_total+val_2; +end +val_1 = d_1_total/numSamplesToAvg; +val_2 = d_2_total/numSamplesToAvg; +d_2 = (val_2+b)/a; +d_2 = ((1/d_2)-.42)/2.54; +d_1 = (val_1+b)/a; +d_1 = ((1/d_1)-.42)/2.54; +%%compute offset +heading = (pi()/2) - (atan((2*ys)/abs(d_2-d_1))); +x_offset(i) = (cos(atan(ys/xs)-heading)*sqrt((xs^2) + (ys^2)))+(cos(heading)*(d_1+d_2)/2); +end +figure +plot(x_offset); +axis([1 numOffsetSamples 8-2 8+2]) diff --git a/SimplifiedWallMathV2.m b/SimplifiedWallMathV2.m new file mode 100644 index 0000000..c80df3c --- /dev/null +++ b/SimplifiedWallMathV2.m @@ -0,0 +1,43 @@ +clear all; +close all; +numOffsetSamples = 100; +x_offset_total = 0; +x_offset_total_two = 0; +for i = 1:numOffsetSamples +%% setup sensor accuracy +a = 4417.7766959627; +b = -66.1650874647; +std = 37.03631579; +%% setup robot params +xs = 4.5; %inch +ys = 3; +%% setup sensor readings +d_1 = 10;%3.5;%3.5; %inch +d_2 = 3;%3.5; %inch +d_1_total = 0; +d_2_total = 0; +val_1 = ((1/((d_1*2.54)+.42))*a) - b; +val_1 = normrnd(val_1,std,1,1); +val_2 = ((1/((d_2*2.54)+.42))*a) - b; +val_2 = normrnd(val_2,std,1,1); +d_2 = (val_2+b)/a; +d_2 = ((1/d_2)-.42)/2.54; +d_1 = (val_1+b)/a; +d_1 = ((1/d_1)-.42)/2.54; +d_1_out(i) = d_1; +d_2_out(i) = d_2; +%%compute offset +x_offset_total = x_offset_total+ ((2*ys)/sqrt((2*ys)^2+(d_2-d_1)^2))*(((d_2+d_1)/2)+xs); +x_offset(i) = x_offset_total/i; +end +figure +plot(x_offset); +axis([1 numOffsetSamples x_offset(numOffsetSamples)-2 x_offset(numOffsetSamples)+2]) +figure +plot(d_1_out,'*'); +hold on; +plot(10*ones(1,numOffsetSamples)); +figure +plot(d_2_out,'*'); +hold on; +plot(3*ones(1,numOffsetSamples)); diff --git a/SinCosTest/SinCosTest.ino b/SinCosTest/SinCosTest.ino new file mode 100644 index 0000000..ea9d970 --- /dev/null +++ b/SinCosTest/SinCosTest.ino @@ -0,0 +1,28 @@ +void setup() { + // put your setup code here, to run once: + Serial.begin(115200); + Serial.println("Sys start"); +} + +void loop() { + float y = 0; + long long t = micros(); + for(float i = 0; i < 3000;i++) + { + y = sin(i); + } + Serial.print("sin "); + Serial.print(y); + Serial.print('\t'); + Serial.println((micros()-t)/3000.0); + y = 0; + t = micros(); + for(float i = 0; i < 3000;i++) + { + y = cos(i); + } + Serial.print("cos "); + Serial.print(y); + Serial.print('\t'); + Serial.println((micros()-t)/3000.0); +} diff --git a/libraries/.gitattributes b/libraries/.gitattributes new file mode 100644 index 0000000..bdb0cab --- /dev/null +++ b/libraries/.gitattributes @@ -0,0 +1,17 @@ +# Auto detect text files and perform LF normalization +* text=auto + +# Custom for Visual Studio +*.cs diff=csharp + +# Standard to msysgit +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain diff --git a/libraries/.gitignore b/libraries/.gitignore new file mode 100644 index 0000000..96374c4 --- /dev/null +++ b/libraries/.gitignore @@ -0,0 +1,43 @@ +# Windows image file caches +Thumbs.db +ehthumbs.db + +# Folder config file +Desktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msm +*.msp + +# Windows shortcuts +*.lnk + +# ========================= +# Operating System Files +# ========================= + +# OSX +# ========================= + +.DS_Store +.AppleDouble +.LSOverride + +# Thumbnails +._* + +# Files that might appear on external disk +.Spotlight-V100 +.Trashes + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk