//

// 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 = 1380;
const double ServoScale_us = 8.0;
// micro-seconds per degree
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
//////////////////////////////////////////////////////////////////
ServoDeg = 0.2*JoystickPercent; // pass Joystick command only
//////////////////////////////////////////////////////////////////
// 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);

//
// 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
//
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);
}