Permalink
Browse files

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

  • Loading branch information...
tylercrumpton committed Mar 27, 2012
1 parent c15dbca commit 957227093cb4feee28d8f7251e9efb97f031f9f0
Showing with 146 additions and 167 deletions.
  1. +94 −49 robot/robot.ino
  2. +52 −118 robot/sensor_read.ino
View
@@ -1,49 +1,88 @@
+//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)
// 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;
}
}
Oops, something went wrong.

0 comments on commit 9572270

Please sign in to comment.