Skip to content

Commit

Permalink
cleaned up code for final report
Browse files Browse the repository at this point in the history
  • Loading branch information
@damped
damped committed May 6, 2016
1 parent cc1892a commit d11600f
Show file tree
Hide file tree
Showing 6 changed files with 103 additions and 142 deletions.
Binary file added balancing_filter.pdf
View file
Binary file not shown.
27 changes: 14 additions & 13 deletions src/imu.cpp
View file
@@ -1,20 +1,19 @@

//authors Michael and Jonas
//description, Program to return an accurate angle from the IMU.
// Authors Michael and Jonas
// Description, Program to return an accurate angle from the IMU.
//
//Outputs
// Outputs angles in degrees


#include "imu.h"

//global constants
#define PI 3.14159265359 // Its pi day... need at least 11 decimals
//#define ACCELEROMETER_SENSITIVITY 2047.97 //+-16 g full scale range of accelerometer
#define ACCELEROMETER_SENSITIVITY 1000 //+-16 g full scale range of accelerometer
#define GYROSCOPE_SENSITIVITY 1/14.375 //LSB/(deg/s)
#define dt 0.01 //sampling rate 0.01 = 10ms, need to add function input for this
#define UPPER_ACC_FORCE 4095.94 // max force 2g*2047.97
#define PIDIV 0.31831 //1/PI because multiplication is faster
//Global constants
#define PI 3.14159265359 // Its pi day... need at least 11 decimals
//#define ACCELEROMETER_SENSITIVITY 2047.97 //+-16 g full scale range of accelerometer
#define ACCELEROMETER_SENSITIVITY 1000 //+-16 g full scale range of accelerometer
#define GYROSCOPE_SENSITIVITY 1/14.375 //LSB/(deg/s)
#define dt 0.01 //sampling rate 0.01 = 10ms, need to add function input for this
#define UPPER_ACC_FORCE 4095.94 // max force 2g*2047.97
#define PIDIV 0.31831 //1/PI because multiplication is faster

// function to configure the gyroscope
int gyroConfig(){
Expand Down Expand Up @@ -85,7 +84,7 @@ void accPitch(float *aPitch, int devAccel){
//printf("pitch = %lf",*aPitch);

} while (forceMagnitudeApprox > ACCELEROMETER_SENSITIVITY && forceMagnitudeApprox < 32768);

}

void gyroPitch(float *gyrPitch, int devGyro)
Expand All @@ -110,6 +109,8 @@ void gyroPitch(float *gyrPitch, int devGyro)
*gyrPitch = (Yg/* * GYROSCOPE_SENSITIVITY*/); // Angle around the X-axis
}

// Get a new angle in degrees
// Accepts the old pitch (for intigration) and device id of the accel and gyro
void getAngle(float *pitch, int devAccel, int devGyro)
{
float aPitch,gyrPitch;
Expand Down
120 changes: 37 additions & 83 deletions src/rollie.cpp
View file
@@ -1,20 +1,20 @@
/*************************************************************
*
* rollie.cpp - Self balencing robot project
* rollie.cpp - Self balencing robot project
*
* Authours: Jonas Menge and Michael Jones
* Authours: Jonas Menge and Michael Jones
*
* Usage:
* Usage: sudo ./rollie
*
*
*************************************************************/
#include "imu.h"
#include "pid.h"
#include "stepper.h"
#include "imu.h" // Inertial measurment unit for getting angles
#include "pid.h" // PID Controller
#include "stepper.h" // Stepper motor controller

#include <stdio.h>
#include <wiringPi.h>
#include <thread>
#include <wiringPi.h> // For interfacing whith the Raspberry Pi hardware
#include <thread> // To run concurrent pocesses
#include <math.h>

#define DEADBAND 0.0 // Absolute amount of angle error that is exeptable
Expand All @@ -25,76 +25,37 @@ void loop(pid_filter_t *pidAngle, pid_filter_t *pidPos, int devAccel, int devGyr

int main()
{
/* Angle PID controller setup */
// Angle PID controller setup
pid_filter_t pidAngle;
pid_init(&pidAngle);

/* Position PID controller setup */
// Position PID controller setup
pid_filter_t pidPos;
pid_init(&pidPos);


/* Set the pid frequency */
// Set the pid frequency
pid_set_frequency(&pidPos, 100);
pid_set_frequency(&pidAngle, 100);

/* Set the maximum positinal PID limit */
// Set the maximum positinal PID limit
pid_set_integral_limit(&pidPos,1428571.0); // 10 degrees max

/* Preset the integral */
// Preset the integral
pid_set_integral(&pidPos, 857142.8); // set to 6 degrees

// pid_set_gains(&pid, 0.026, /*0.0000108*/0.0, 0.000024);
// pid_set_gains(&pid, 0.020, /*0.0000108*/0.0, 0.00004);
// pid_set_gains(&pid, 0.027,/* 0.0000808*/0.0, /*0.000024*/ 0.001);
// pid_set_gains(&pid, 0.029,/* 0.0000808*/0.0, /*0.000024*/ 0.001);
// pid_set_gains(&pid, 0.050,/* 0.0000808*/0.0, /*0.000024*/ 0.002);
// pid_set_gains(&pid, 0.20,/* 0.0000808*/0.00001, /*0.000024*/ 0.003);
//pid_set_gains(&pidAngle, 0.260,/* 0.0000808*/0.0000, /*0.000024*/ 0.000010);
//pid_set_gains(&pidPos, -0.0000000000,/* 0.0000808*/0.000007, /*0.000024*/ 0.05);

// Tune the PID controller
pid_set_gains(&pidAngle, 0.299, 0.0000, 0.000030);
pid_set_gains(&pidPos, 0.0, 0.00082, 0.00005);

// Start Stepper Motor Thread
stepper stepper; // Create stepper struture
setSpeed(0.0, &stepper.period); // Preset the period to 0.0


/* IMU setup */
int devAccel = accConfig();
int devGyro = gyroConfig();
/* float pitch = ;
for (size_t i = 0; i < 200; i++) {
getAngle(&pitch,devAccel,devGyro); // Get an agle to clear out the crap
}
*/

/* Start Stepper Motor Thread */
stepper stepper;
setSpeed(0.0, &stepper.period);

std::thread t_stepper;
std::thread t_stepper; // Create the stepper thead
t_stepper = std::thread(stepperControl, &stepper);

//bool enable = 1;
/* Motor tester eh
while (1){
printf("Set velocity to 0.01 m/s\n");
setSpeed(0.01, &period);
delay(10000);
printf("Set velocity to 0.02 m/s\n");
setSpeed(0.02, &period);
delay(10000);
printf("Set velocity to 0.03 m/s\n");
setSpeed(0.03, &period);
delay(10000);
printf("Set velocity to 0.04 m/s\n");
setSpeed(0.04, &period);
delay(10000);
printf("Set velocity to 0.05 m/s\n");
setSpeed(0.05, &period);
delay(10000);
}
*/

// Balencing loop
loop(&pidAngle, &pidPos, devAccel, devGyro, &stepper);


Expand All @@ -111,46 +72,39 @@ void loop(pid_filter_t *pidAngle, pid_filter_t *pidPos, int devAccel, int devGyr
float setpointAngle = 0.0;
float pidOutput = 0.0;
float p0,p1 = 0.0;
//for(int i = 0; i >= 10; i++) { getAngle(&pitch,devAccel,devGyro); }

while (1){
// time = micros();

p0 = pitch;
// Simple avreraging
p0 = pitch;
getAngle(&pitch,devAccel,devGyro);
p1 = pitch;
p1 = pitch;
getAngle(&pitch,devAccel,devGyro);
pitch = (pitch + p0 + p1) / 3.0;


//printf("\rGA: %u ", (micros()-time));

//if(abs(error) < DEADBAND){
// error = 0;
//}
pitch = (pitch + p0 + p1) / 3.0;

// Limit positinal range to prevent interator from overflowing too quikly
if (500 < stepper->count)
{
stepper->count = 500;
} else if (-500 > stepper->count) {
stepper->count = -500;
}

/////////// Get and set the frequency
//pid_set_frequency(pidPos, (float)(micros() - pidTimer));
//pid_set_frequency(pidAngle, (float)(micros() - pidTimer));
//printf("%f", (float)(micros() - pidTimer));
//pidTimer = micros();

if (500 < stepper->count)
{
stepper->count = 500;
} else if (-500 > stepper->count) {
stepper->count = -500;
}
//////////// PID Controller ///////////

// Error = Positional setpoint - actual position
errorPos = setpointPos - stepper->count;
setpointAngle = pid_process(pidPos, errorPos);
// Outputs the setpint angle for the next PID

// Errorr = Anguar setpoint - acutal angle
errorAngle = setpointAngle - pitch;
pidOutput = pid_process(pidAngle, errorAngle);
// Outputs the motor velocity in m/s

// Calculate and set the motor speed
setSpeed(pidOutput, &stepper->period);
//printf("\r Comp Angle: %0.2f", pitch);
//printf("\r Comp Angle: %0.2f", pitch);
printf("\rerPos = %f, spAngle = %f, ActualA = %f, erAngle = %f, PID = %f",errorPos, setpointAngle, pitch, errorAngle, pidOutput);

}
Expand Down
95 changes: 51 additions & 44 deletions src/stepper.cpp
View file
@@ -1,3 +1,11 @@
/* stepper.cpp
* Written by Jonas Menge and Michael Jones
*
* Accepts the period between steps
*
* Returns curren step count
*/

#include "stepper.h"
#define RAMP 0.02

Expand All @@ -9,63 +17,62 @@ const int motor2Dir = 22;


void stepperControl(stepper *stepper){
motorSetup();

while(1){
if (stepper->period != 0.0){
// Do math with the rate to scail it and set dir
//printf("period = %f",*period);
if (stepper->period > 0){
digitalWrite(motor1Dir, HIGH);
digitalWrite(motor2Dir, HIGH);
stepper->count--;
} else {
digitalWrite(motor1Dir, LOW);
digitalWrite(motor2Dir, LOW);
stepper->count++;
}


// if (*period > time) {
// time = time + RAMP;
// } else if (*period < time) {
// time = time - RAMP;
// }

digitalWrite(motor1Step, HIGH);
digitalWrite(motor2Step, HIGH);
wait(&stepper->period);
digitalWrite(motor1Step, LOW);
digitalWrite(motor2Step, LOW);
wait(&stepper->period);
// printf("\r %f",*period);
} else {
delay(5);
motorSetup();

while(1){
if (stepper->period != 0.0){
// Do math with the rate to scail it and set dir
//printf("period = %f",*period);
if (stepper->period > 0){
digitalWrite(motor1Dir, HIGH);
digitalWrite(motor2Dir, HIGH);
stepper->count--;
} else {
digitalWrite(motor1Dir, LOW);
digitalWrite(motor2Dir, LOW);
stepper->count++;
}

// Do the step
digitalWrite(motor1Step, HIGH);
digitalWrite(motor2Step, HIGH);
wait(&stepper->period);
digitalWrite(motor1Step, LOW);
digitalWrite(motor2Step, LOW);
wait(&stepper->period);

} else {
delay(5);
}
}
}
}


// Initialize output pins
void motorSetup(){
printf("starting motor thread");
wiringPiSetupGpio(); // Initialize wiringPi -- using Broadcom pin numbers
printf("starting motor thread");
wiringPiSetupGpio(); // Initialize wiringPi -- using Broadcom pin numbers

pinMode(motor1Step, OUTPUT);
pinMode(motor2Step, OUTPUT);
pinMode(motor1Step, OUTPUT);
pinMode(motor2Step, OUTPUT);

pinMode(motor1Dir, OUTPUT);
pinMode(motor2Dir, OUTPUT);
pinMode(motor1Dir, OUTPUT);
pinMode(motor2Dir, OUTPUT);


}


// Alow updating the time while waiting
void wait(float *time){
//printf("wait: %f",*time);
for (float i = -0.01; i <= abs(*time/2.0); i = i + 10.0) // check how long we need to wait
{
delayMicroseconds(10);
}
//printf("wait: %f",*time);
for (float i = -0.01; i <= abs(*time/2.0); i = i + 10.0) // check how long we need to wait
{
delayMicroseconds(10);
}
}

// Calculate the new motor speed and store it
void setSpeed(float velocity, float *pulseTimePtr) {
*pulseTimePtr = 1000000*(1.0 / (MOTOR_STEPS / (MICRO_STEPS * DIA * PI))) / velocity;
//printf("Period: %f, velocity: %f\n", *pulseTimePtr,velocity);
Expand Down
3 changes: 1 addition & 2 deletions src/stepper.h
View file
Expand Up @@ -11,7 +11,7 @@
#define MICRO_STEPS 0.5 // Micro steps (half steps = 0.5)
#define DIA 0.085 // Daiameter in m

#define PI 3.14159265359 // Its pi day... need at least 11 decimals
#define PI 3.14159265359 // Its pi day... need at least 11 decimals


// Structs
Expand All @@ -26,5 +26,4 @@ void wait(float *time); // Wait loop
void stepperControl(struct stepper *stepper);
void setSpeed(float velocity, float *pulseTimePtr);


#endif
Binary file added tuning_a_PID_loop.pdf
View file
Binary file not shown.

0 comments on commit d11600f

Please sign in to comment.