//PD CONTROL

// University of Pennsylvania
// ESE 505 - 2015C
// Project 2
// OneTwoRedBlue.ino
//
// Headers contain device drivers
//
#include <SPI.h>
#include <Pixy.h>
#include <Servo.h>
//
// Pixy is an object type defined in Pixy header file
// Servo is an object type defined in Servo header file
//
Pixy PingPongPixy;
Servo PingPongServo;
//
// here are the pins used
//
const int PingPongServoPin = 3;
const int JoystickPin = A0;
//
// servo constants -- YOU MAY CHANGE THESE TO GET GOOD CALIBRATION
//
const int ServoCenter_us = 1405;
const double ServoScale_us = 8.0;
// micro-seconds per degree
double Ymeasured = 0;
double Ymeasured_previous = 0;
double error;
double error_previous;
double Joystick_previous = 0;
double u ;
double y_prev = 0;
double y_new;
double a=0.1;
double error_sum=0;
double set_position;
int i=0;
int cycle=0;
double pos_array[4]={-40, 40, -80, 80};

void setup()
{
//
// Serial baud rate -- faster is usually better
//
Serial.begin(57600);
//
// Servo attached to Arduino digital pin 3
//
PingPongServo.attach(3);
PingPongServo.writeMicroseconds(ServoCenter_us);
//
// initialize the Pixy camera

//
PingPongPixy.init();
}
void loop()
{
static unsigned long microsLast = 0; // elapsed time at end of last call to lo
op
double deltaT = 0.02; // time between digital updates (20ms = PIXY & Servo upd
ate rates)
word JoystickRaw = 512;
double JoystickPercent = 0.0;
static int xBallRawLast = 2; // start with max left position as indication of
failed PIXY read
int xBallRaw;
double xBallmm = 0.0;
double ServoDeg = 0.0;
double t_us;
boolean ServoDisabled;
//
// get the joystick position
// Convert to +/- 100% relative to center
//
JoystickRaw = analogRead(JoystickPin);
JoystickPercent = -100.0 + (200.0*JoystickRaw)/1023.0;
//
// get the ball position
// negative value indicates no ball found
// convert distance in mm from center of camera field of view
// YOU SHOULD FIX THE DISTANCE CALIBRATION!
//
xBallRaw = PingPongCheck();
if (xBallRaw < 0)
{
xBallRaw = xBallRawLast;
}
else
{
xBallRawLast = xBallRaw;
}
xBallmm = -111.0 + (222.0*xBallRaw)/319.0; // 319 = max pixel value in PIXY c
amera
//////////////////////////////////////////////////////////////////
// Control Law Starts Here
//////////////////////////////////////////////////////////////////
set_position=pos_array[i];
Ymeasured = ((xBallmm + 5)*100/65);
// error = JoystickPercent - Ymeasured;
error = set_position - Ymeasured;
error_sum += error*0.02;
u = (error-error_previous)/(0.02);
y_new = (1-a)*y_prev + a*u; //Filter derivative portion
ServoDeg = 0.018*(error) + (0.05*y_new) + 0.1*(error_sum); // PID control

Ymeasured_previous = Ymeasured;
//Joystick_previous = JoystickPercent;
error_previous = error;
y_prev = y_new;
if((error<5)&&(error>-5))
{cycle=cycle+1;}
else
{cycle=0;}
if(cycle>20)
{i=i+1;
cycle=0;
}
if(i>3)
{i=0;
}

//////////////////////////////////////////////////////////////////
// Control Law Ends Here
//////////////////////////////////////////////////////////////////
//
//
//
//
//
//

convert ServoDeg to MicroSeconds

then send the command to the servo
smaller values of t_us tilt right (positive sense of ServoOne)
DON'T CHANGE LIMITS IN CONSTRAIN FUNCTION -- CAN DAMAGE SERVO
t_us = constrain(ServoCenter_us - ServoScale_us * ServoDeg, 1000, 1800);
PingPongServo.writeMicroseconds(t_us);

//
// now send serial information
//
Serial.print(JoystickPercent,1);
Serial.print(',');
Serial.print(xBallmm,1);
Serial.print(',');
Serial.println(ServoDeg,1);
Serial.print(',');
Serial.println(Ymeasured,1);
Serial.print(',');
Serial.println(error,1);
Serial.print(',');
Serial.println(error_sum,1);
//
// force constant frame rate
//
while (micros() < microsLast + deltaT*1000000); // wait for deltaT since last
time through
microsLast = micros(); // save value for next time through

}
//
// modified from online library
// assumes only 1 block is reported
// that block must be the Ping Pong Ball
//
//void PID (double set_pos) { }
word PingPongCheck()
{
int xpos=-1;
uint16_t nblocks;
nblocks = PingPongPixy.getBlocks();
if (nblocks)
{
xpos = PingPongPixy.blocks[0].x;
}
return xpos;
}
void SimPlot(int data1, int data2, int data3, int data4)
{
int pktSize;
int buffer[20];
buffer[0] = 0xCDAB;
//SimPlot packet header. Indicates start of da
ta packet
buffer[1] = 4*sizeof(int);
//Size of data in bytes. Does not include the
header and size fields
buffer[2] = data1;
buffer[3] = data2;
buffer[4] = data3;
buffer[5] = data4;
pktSize = 2 + 2 + (4*sizeof(int)); //Header bytes + size field bytes + data
Serial.write((uint8_t * )buffer, pktSize);
}