# 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 <ArduinoSort.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

//Set up LED object

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
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++)
{
}
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()
{