Demo entry 6360479

Ball and Beam Code

   

Submitted by anonymous on May 01, 2017 at 19:50
Language: C. Code size: 6.6 kB.

//////////////////////////////////////////////////////////////////
//
//  Ball and Beam
//  Stefan Colton and Eric Chan
//
//  5/1/17
//
//////////////////////////////////////////////////////////////////


// Includes
#include <Math.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <ArduinoSort.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
#include <Adafruit_NeoPixel.h>


// PIN definitions
#define IR_input A0
#define POT_input A2
#define LED_output 6
#define encoderPinA 2
#define encoderPinB 3

// Filter constants
#define FILTER_K 0.70
#define MEDFILT 9

//////////////////////////////////////////////////////////////////
        // TUNABLE VALUES

double motorP = 4;
double motorI = 0;
double motorD = 0.1;

double positionP = 1.0;
double positionI = 0.00;
double positionD = 40.0;



//////////////////////////////////////////////////////////////////
        // SAFETY CONSTANTS

const double maxSpeed = 255;  // Limit the motor speed
const double minPos = -150;   // Set maximum and minimum deflection angles
const double maxPos = 150;    // so that the beam doesn't break anything
                              // if something goes wrong. These correspond
                              // to +-30ยบ on the beam

//////////////////////////////////////////////////////////////////


double deg = 0;   // motor angle in degrees
double N = 1024;  // Number of ticks per rotation on the encoder
volatile double encoderPos = (deg/360.)*N;
double pot = 0;   // Raw potentiometer value (0 - 1024)


int motorTarget = 0;  // The target angle of the motor
int v;                // The target speed of the motor

double rawPosition = 0;       // Raw position of the ball.
double f_IRDistance_cm; // The filtered IR distance in centimeters
double velocity;        // Velocity of the ball
double ballTarget = 20; // Position target of the ball in centimeters.
                        // Ranges from 10 to 50 cm.

double ballIntegral = 0;  // Integrated error of the ball's position for PID tuning
double motorIntegral = 0; // Integrated error of the motor's position


int MedFiltArray[MEDFILT];    // Array for median filtering
double last_f_IRDistance_cm = 0;    // Save the last value of position for velocity calculations

int t = 0;  // Counts how many loops we've done. Increments by 1 for every cycle.
int calibration = 1;  // If 1, use PID, else use only PD. Used to find the equilibrium
                      // position of the ball.






// Set up motor objects
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);

//Set up LED object
Adafruit_NeoPixel strip = Adafruit_NeoPixel(16, LED_output, NEO_KHZ800 + NEO_RGB);







void setup()
{
  Serial.begin(9600);

  // Encoders
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  digitalWrite(encoderPinA, HIGH); // Pull Up Resistor
  digitalWrite(encoderPinB, HIGH);
  attachInterrupt(0, doEncoder, CHANGE);

  // IR
  pinMode(IR_input, INPUT);

  // Motor
  AFMS.begin();
  myMotor->run(RELEASE);

  // LED
  strip.begin();
  strip.show();

  // Wait 10 seconds before starting anything.
  // Useful for testing.
  delay(10000);
}


//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////

void loop()
{
  // Calibration Step: after 20 seconds, disable integral
  // and save current position as the new zero
  if (calibration == 1 && millis() > 20000)
  {
    positionI = 0.00;
    encoderPos = 0;
    integral = 0;
  }


  // Read and filter Potentiometer data
  pot = analogRead(POT_input);
  ballTarget = FILTER_K * ballTarget + (1 - FILTER_K) * map(pot, 0, 1024, 10, 50);

  // Read and filter IR data
  for (int i = 0; i < MEDFILT; i++)
  {
    MedFiltArray[i] = analogRead(IR_input);
  }
  sortArray(MedFiltArray, MEDFILT);
  rawPosition = pow(10,log10( (5.00/1024) * (double)MedFiltArray[MEDFILT/2 + 1] / 15.998) / -0.839);
  f_IRDistance_cm = FILTER_K * f_IRDistance_cm + (1 - FILTER_K) * rawPosition;


  // LED output. Set the LED's once every 1000 cycles so we don't suck up too much processing power.
  if (t % 1000 == 0)
  {
    double ledPos = map(f_IRDistance_cm, 65, -5, 0, 16);
    for (int i = 0; i < 16; i++)
    {
        if (i > ledPos)
        {
          strip.setPixelColor(i, 255, 0, 0);  // Set current ball position to Green
        }
        else
        {
          strip.setPixelColor(i, 255, 255, 255);
        }
    }
    strip.setPixelColor(map(ballTarget, 65, -5, 0, 16), 0, 255, 0); // Set target position to Red
    strip.show();

  }


  // Calculate velocity and apply exponential smoothing
  velocity = 0.5 * velocity + (1 - 0.5) * (f_IRDistance_cm - last_f_IRDistance_cm);

  // Retrieve Motor position in degrees
  deg = (encoderPos / N) * 360.00;


  // First PID loop: Find the desired angle of the motor as a function of the ball's position, velocity,
  // and integrated error.
  motorTarget = PID(ballTarget, f_IRDistance_cm, &ballIntegral, velocity, positionP, positionI, positionD);

  // Second PID loop: Find the desired motor speed as a function of the motor's speed, velocity, and integrated error.
  v = PID(-motorTarget, deg, &motorIntegral, 0, motorP, motorI, motorD);
  setMotor(v);


  // Plot response graphs
  Serial.print(0);
  Serial.print("\t");
  Serial.print(50);
  Serial.print("\t");
  Serial.print(ballTarget);
  Serial.print("\t");
  Serial.println(f_IRDistance_cm);

  last_f_IRDistance_cm = f_IRDistance_cm;
  delay(1);
}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////


// Takes a target, position, integrated error, velocity, and PID constants and
// generates an output signal.
int PID(double target, double y, double *iy, double dy, double kp, double ki, double kd)
{
  double error = target-y;
  double P = error * kp;
  *iy += error;
  double I = *iy * ki;
  double D = -dy * kd;

  int output = (int) constrain(P + I + D, -255, 255);
  return output;
}

// Helper function to allow for out of range motor speeds and
// direction changes.
void setMotor(double v)
{
  if (v > maxSpeed)
    v = maxSpeed;
  if (v < -maxSpeed)
    v = -maxSpeed;
  if (v >= 0)
  {
    myMotor->run(FORWARD);
    myMotor->setSpeed(v);
  }
  else
  {
    myMotor->run(BACKWARD);
    myMotor->setSpeed(-v);
  }
}

// Interrupt function to find motor position
void doEncoder()
{
  if (digitalRead(encoderPinA) == digitalRead(encoderPinB))
  {
    encoderPos--;
  }
  else
  {
    encoderPos++;
  }
}

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).