Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Updated code for new motors with encoders. Pseudo-stable.

  • Loading branch information...
commit 957227093cb4feee28d8f7251e9efb97f031f9f0 1 parent c15dbca
@tylercrumpton authored
Showing with 146 additions and 167 deletions.
  1. +94 −49 robot/robot.ino
  2. +52 −118 robot/sensor_read.ino
View
143 robot/robot.ino
@@ -1,34 +1,70 @@
+//INCLUDES----------------------------------------------------------
+#include <PinChangeInt.h>
+#include <AdaEncoder.h>
#include <PID_v1.h>
+#include <TimerOne.h>
+#include <math.h>
+//Constants---------------------------------------------------------
+//Define Motor Controller Pins
#define LEFT_EN_PIN 5
#define LEFT_L4_PIN 10
#define LEFT_L3_PIN 7
#define RIGHT_EN_PIN 11
#define RIGHT_L2_PIN 12
-#define RIGHT_L1_PIN 6
-
+#define RIGHT_L1_PIN 6
+
+//Define Motor Encoder Pins
+#define L_PINA A0
+#define L_PINB A1
+#define R_PINA A2
+#define R_PINB A3
+
+//Define Sensor Pins
+#define gyroPin A4
+#define accelPin A5
+
+//Define Constants
+#define pi 3.14159;
+#define samplePeriod 20000 //Time in uS for sampling loop period.
+#define MAX_TILT 10 // Maximum controllable tilt in degrees
+#define MAX_MOTOR_SPEED 255 // Maximum PWM for motor control
+//Global Variables---------------------------------------------------
+//Encoder Counts
+int L_Clicks, R_Clicks = 0;
-#define MAX_TILT 30 // Maximum controllable tilt
-#define MAX_MOTOR_SPEED 255 // Maximum PWM for motor control
-#define MAX_TURN_SPEED 25 // Maximum PWM differntial for turning
- // (NOTE: MAX_TURN_SPEED determines max balance speed.)
+//Encoder Pointers (set by genie fuction in AdaEncoder library)
+int8_t clicks=0;
+char id=0;
+//Balance Variables
+double angle = 0; //Filtered angled value
-
-double angle = 0; //Filtered angled value
+double accelVal = 0; //Raw accel value from ADC 0-1023
+double gyroVal = 0; //Raw gyro value from ADC 0-1023
+
+int accelOffset = 0; //Raw offset for accelerometer
+int gyroOffset = -8; //Raw offset for gyro
+
+double L_clicks_last = 0; //Encoder clicks last cycle
+double R_clicks_last = 0;
// PID Coeffs
-double KP = 100.0;
-double KI = 22.0;
-double KD = 0.05;
+double KP = 70; //100
+double KI = 400; //22
+double KD = .006; //.05
// Control Coeffs
-double CP = 0.0010;
-double CI = 0.75;
-double CD = 0.0 ;
+double CP = .0; //.0010
+double CI = 0 ; //.75
+double CD = 0.000 ; //.0
+// Turn PID Coeffs
+double TP = 0; //250
+double TI = 00; //500
+double TD = 0; //0.09
double desiredTilt; // Desired tilt of robot (+/- MAX_TILT)
-int desiredSpeed; // Desired speed of robot (+/- MAX_MOTOR_SPEED)
+double desiredSpeed; // Desired speed of robot (+/- MAX_MOTOR_SPEED)
double balanceSpeed; // Base motor speed for balance (+/- MAX_MOTOR_SPEED)
int turnSpeed; // Turn speed differential of robot (+/- MAX_TURN_SPEED)
@@ -36,14 +72,17 @@ int turnSpeed; // Turn speed differential of robot (+/- MAX_TURN_SPEED)
// Setup for PID computation
double setpointPID, inputPID, outputPID;
PID bPID(&inputPID, &outputPID, &setpointPID, KP, KI, KD, DIRECT);
-
+
double setpointCPID, inputCPID, outputCPID;
PID cPID(&inputCPID, &outputCPID, &setpointCPID, CP, CI, CD, DIRECT);
+double setpointTPID, inputTPID, outputTPID;
+PID TPID(&inputTPID, &outputTPID, &setpointTPID, TP, TI, TD, DIRECT); //turn correction pid function
+
void setup()
{
- Serial.begin(9600);
+ Serial.begin(115200);
// Initialize variables.
turnSpeed = 0;
@@ -62,53 +101,59 @@ void setup()
// Turn on the PID.
bPID.SetMode(AUTOMATIC);
cPID.SetMode(AUTOMATIC);
+
+ TPID.SetMode(AUTOMATIC);
+
// Force PID to the range of motor speeds. (+/- MAX_MOTOR_SPEED - MAX_TURN_SPEED)
//bPID.SetOutputLimits(-(MAX_MOTOR_SPEED - MAX_TURN_SPEED), MAX_MOTOR_SPEED - MAX_TURN_SPEED);
- bPID.SetOutputLimits(-255,255);
+ bPID.SetOutputLimits(-MAX_MOTOR_SPEED,MAX_MOTOR_SPEED);
bPID.SetSampleTime(2);
- cPID.SetOutputLimits(-25,25);
+ cPID.SetOutputLimits(-MAX_TILT,MAX_TILT);
cPID.SetSampleTime(2);
+
+ TPID.SetOutputLimits(-15,15);
+ TPID.SetSampleTime(2);
+
initSensors();
+
+ //Setup motor encoders
+ AdaEncoder::addEncoder('a', L_PINA, L_PINB);
+ AdaEncoder::addEncoder('b', R_PINA, R_PINB);
}
void loop()
{
-
- //balance();
-}
-
-// Calculate balanceSpeed given desired speed.
-int calcPID(double input, int target)
-{
- inputPID = input; // Set PID input to tilt angle.
- setpointPID = (balanceSpeed - target) * CP;
- bPID.Compute(); // Compute correction, store in outputPID.
- balanceSpeed = outputPID;
+ encoder *thisEncoder;
+ thisEncoder=AdaEncoder::genie(&clicks, &id);
+ if (thisEncoder != NULL) {
+ //thisEncoder=AdaEncoder::getFirstEncoder();
+
+ if (id =='a')
+ {
+ L_Clicks += clicks;
+ }
+ if (id =='b')
+ {
+ R_Clicks += clicks;
+ }
+ thisEncoder->clicks = 0;
+ }
+ /*Serial.print(L_Clicks);
+ Serial.print("\t");
+ Serial.println(R_Clicks);*/
}
-void balance()
-{
- // Read the sensors and calculate a tilt angle.
- //double angle = getCurrentTilt();
-
- // Try to balance the robot at the desired angle.
-
-
- // Update the motor PWM values.
- updateMotors();
-}
-
// Converts the value between +/- MAX_MOTOR_SPEED to an actual PWM signal and direction
void updateMotors()
{
- double tempLeftSpeed = (balanceSpeed - turnSpeed); // Left motor speed from -255 to 255
- double tempRightSpeed = (balanceSpeed + turnSpeed)*0.8; // Reft motor speed from -255 to 255
+ double tempLeftSpeed = (balanceSpeed - turnSpeed-outputTPID); // Left motor speed from -255 to 255
+ double tempRightSpeed = (balanceSpeed + turnSpeed+outputTPID); // Reft motor speed from -255 to 255
analogWrite(LEFT_EN_PIN, byte(abs(tempLeftSpeed))); // Set PWM as magnitude of left speed (0 to 255)
- analogWrite(RIGHT_EN_PIN, byte(abs(tempRightSpeed))); // Set PWM as magnitude of right speed (0 to 255)
+ analogWrite(RIGHT_EN_PIN, byte(abs(tempRightSpeed))*.85); // Set PWM as magnitude of right speed (0 to 255)
if (tempLeftSpeed < 0)
{
@@ -124,13 +169,13 @@ void updateMotors()
if (tempRightSpeed < 0)
{
- digitalWrite(RIGHT_L1_PIN, LOW); // If negative, direction = LOW;
- digitalWrite(RIGHT_L2_PIN, HIGH); // If negative, direction = LOW;
+ digitalWrite(RIGHT_L1_PIN, HIGH); // If negative, direction = LOW;
+ digitalWrite(RIGHT_L2_PIN, LOW); // If negative, direction = LOW;
}
else
{
- digitalWrite(RIGHT_L1_PIN, HIGH); // If positive, direction = HIGH
- digitalWrite(RIGHT_L2_PIN, LOW); // If negative, direction = LOW;
+ digitalWrite(RIGHT_L1_PIN, LOW); // If positive, direction = HIGH
+ digitalWrite(RIGHT_L2_PIN, HIGH); // If negative, direction = LOW;
}
}
View
170 robot/sensor_read.ino
@@ -1,31 +1,6 @@
-//WORK IN PROGRESS
-//Implements a complimetary filter to combine accelerometer and gyro readings into to one
-//angle in degrees/
-/*Constraints
--Must allow time for initial stabilization of filtered angle 3-4 sec
--Extremem Rapid horizonal acceleraton will result in an incorrect angle for a small period of time
-*/
-#include <TimerOne.h>//<-------THIRD PARTY LIBRARY. DOWNLOAD FROM ARDUINO WEBSITE TO RUN
-
-//Pin definitions and constants
-const double pi=3.14159;
-const int gyroPin = A4;
-const int accelPin = 3;
-const double samplePeriod = 20000; //Time in uS for sampling loop period.
-int tiltOffset = 0;
-
-
-int accelVal = 0; //Raw accel value in width of high pulse from PWM
-int gyroVal = 0; //Raw gyro value from ADC 0-1023
-int highTime,lowTime = 0; //Used for measuring the accelVal, Must be global for interrupt.
-//double angle = 0; //Filtered angled value
-
-int accelOffset = 0; //Raw offset for accelerometer
-int gyroOffset = 1; //Raw offset for gyro
-
//Variables used for debugging--CAN DELETE
@@ -35,7 +10,7 @@ int timeNow,prevTime = 0;
void initSensors()
{
- attachInterrupt(1,accelGetValue, CHANGE); //Interrupt setup on pin 2--triggers on both edges for accel
+ //attachInterrupt(1,accelGetValue, CHANGE); //Interrupt setup on pin 2--triggers on both edges for accel
Timer1.initialize(samplePeriod); //Time one overflow defines sample period
Timer1.attachInterrupt(sampling_loop); //Interrupr on overflow calls sampling_loop function
}
@@ -44,16 +19,20 @@ void initSensors()
//Currently 50HZ
void sampling_loop()
{
- cli();
+ double L_speed = L_Clicks - L_clicks_last;
+ double R_speed = R_Clicks - R_clicks_last;
+ L_clicks_last = L_Clicks;
+ R_clicks_last = R_Clicks;
+ double avg_speed = ((L_speed+R_speed)/2.0)*.10776; //RPS
double timeScale = 1000000 /samplePeriod; //Compute scalefactor for gyro from degrees per sec.
double gyroAngle = -gyroToDegreesPerSec()/timeScale;
- double accelAngle = -accelToAngle();
- angle = 0.97*(angle + (gyroAngle)) +(0.03*accelAngle); //Compute new angl measurement.
+ double accelAngle = accelToAngle();
+ angle = 0.97*(angle + (gyroAngle)) +(0.0*accelAngle); //Compute new angl measurement.
//DEBUG STATEMENTS
- //angleOnlyG = 1.0*(angleOnlyG + (gyroAngle)) +(0.00*accelAngle);
- //angleOnlyA = 0.0*(angle + (gyroAngle)) +(1.0*accelAngle);
- /*Serial.print(angle);
+ /*angleOnlyG = 1.0*(angleOnlyG + (gyroAngle)) +(0.00*accelAngle);
+ angleOnlyA = 0.0*(angle + (gyroAngle)) +(1.0*accelAngle);
+ Serial.print(angle);
Serial.print("\t");
Serial.print(angleOnlyG);
Serial.print("\t");
@@ -61,44 +40,48 @@ void sampling_loop()
Serial.print("\t");
Serial.println(gyroVal);*/
- //Serial.print(angle);
- //Serial.print("\t");
- //Serial.print(balanceSpeed);
- //Serial.print("\t");
- //Serial.println(setpointPID);
+
+ //inputCPID = balanceSpeed;
-
-// if (balanceSpeed > 0)
-// setpointPID = setpointPID + 0.75;
-// else if (balanceSpeed < 0)
-// setpointPID = setpointPID - 0.75;
-// else
-// setpointPID = setpointPID;
-//
-// if (setpointPID > 25)
-// {
-// setpointPID = 25;
-// }
-// else if(setpointPID < -25)
-// {
-// setpointPID = -25;
-// }
- //setpointPID = (balanceSpeed - 0) * CP;
-
- //setpointPID = 0;
- inputCPID = balanceSpeed;
+ inputCPID = avg_speed;
setpointCPID = desiredSpeed;
cPID.Compute();
+ setpointPID = outputCPID;
inputPID = angle; // Set PID input to tilt angle.
- setpointPID = -outputCPID;
bPID.Compute(); // Compute correction, store in outputPID.
balanceSpeed = outputPID;
+ /*if (balanceSpeed > 0 && balanceSpeed < 40)
+ {
+ balanceSpeed = 40;
+ }
+ if (balanceSpeed < 0 && balanceSpeed > -40)
+ {
+ balanceSpeed = -40;
+ }*/
+
+ setpointTPID = 0;
+ inputTPID = L_speed - R_speed;
+ TPID.Compute();
+
updateMotors();
- sei();
+ Serial.print(angle);
+ Serial.print("\t");
+ Serial.print(setpointPID);
+ Serial.print("\t");
+ Serial.print(angle-setpointPID);
+ Serial.print("\t");
+ /*Serial.print(outputTPID);
+ Serial.println("\t");*/
+ Serial.print(balanceSpeed);
+ Serial.print("\t");
+ Serial.println(avg_speed-desiredSpeed);
+ /*Serial.print("\t");
+ Serial.println(desiredSpeed);*/
+
}
@@ -111,7 +94,14 @@ double getCurrentTilt()
//Takes the raw acceleration PWM signal and converts to a degree. Vertical is 0 degrees.
double accelToAngle()
{
- double accelMilliG = (((accelVal-accelOffset)/10) - 500) * 8;
+ int avg[15] = {0};
+ double sum = 0;
+ for(int i = 0;i<15;i++)
+ {
+ sum += analogRead(accelPin)-accelOffset;
+ }
+ accelVal = sum/15.0;
+ double accelMilliG = (accelVal-338)/.164;
if (accelMilliG > 1000)
{
return 90;
@@ -125,6 +115,8 @@ double accelToAngle()
double accelAngle = asin(accelMilliG/1000)*180/pi;
return accelAngle;
}
+
+
}
//Takes the raw analog value from the acelerometer and converts to degrees per sec.
@@ -134,67 +126,9 @@ double gyroToDegreesPerSec()
gyroVal = analogRead(gyroPin)-gyroOffset;
gyroAnglePerSec = (gyroVal - 512)/1.023;
return gyroAnglePerSec;
-
- //Possible idea to filter out noise.
- /*if (gyroAnglePerSec < 2.0 && gyroAnglePerSec > -2.0)
- {
- return 0;
- }
- else
- {
- return gyroAnglePerSec;
- }*/
}
-//Interrupt driven function to capture the raw accel value. Interrputs on rising and falling edges of the accel Pin
-void accelGetValue()
-{
- cli();
-
- if (digitalRead(accelPin) == HIGH) //Read time of rising edge
- {
- highTime = micros();
- }
- else //Read time of falling edge
- {
- lowTime = micros();
- accelVal = lowTime - highTime; //Calculate width of pulse
- }
- sei();
-}
-/*void GyroCalibrate()
-{
- boolean complete = false;
- int indexNum = 100;
- long val[indexNum] = {0};
- int percentError = 100;
- int index1, index2 = 0;
- double sum, avg = 0;
- while(complete == false)
- {
- if index1 > indexNum;
- {
- index2 = index1 % indexNum;
- }
-
- if (index1 < indexNum || percentError > 5)
- {
- val[index2] = analogRead(A1);
-
- for (int = 0; i < 100; i++)
- {
- sum = sum + val[i];
- }
- avg = sum/indexNum;
-
-
- }
-
- sum = 0;
- index ++;
- }
-}*/
Please sign in to comment.
Something went wrong with that request. Please try again.