Scribd
Upload
127 views3 pages

Ori

ori

Uploaded by

PrakhalGoyal
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
127 views3 pages

Ori

ori

Uploaded by

PrakhalGoyal
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 3

//

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

You might also like