I really need help.
I want to make a eye tracking device with CMUcam5 Pixy camera and bipolar stepper motor which has 200 revolution and works 1.8 degree per steps.
For controlling the stepper motor, I use Arduino Mega and motor shield R3.
I coded for tracking eye.
Pixy camera send y position data of eye location to Arduino controller and this control the motor by the y data.
For example, when a eye move up 10 degree from center, the motor rotate 10 steps to one direction and stop at the position. And then, when the eye move down 15 degree, total -5 degree from center, the motor rotate opposite direction 15 steps.
So, the motor is supposed to stop at proper position through y data of pixy camera.
This video will help to understand what I said.
In the video, the robot is used 4 servo motor to move the eyes, 2 motor for x direction and 2 motor for y direction.
However, I will use just a stepper motor like a y direction motor and it is supposed to move like the eye.
My code problem is motor keeps moving to clockwise or counterclockwise, not stop at proper position like the video.
Also, I will used laser sensor for calibration of steps of motor.
So, my code include laser sensor code with eye tracking code.
Please, help me to figure it out.
#include <Stepper.h>
#include <SPI.h>
#include <Pixy.h>
#include<stdio.h>
using namespace std;
Pixy pixy;
Stepper motor(200,12,13);
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;
// variable to store the stepper position
float y_scale = .2; //control step scale
float y = 0;
int Laser = 6; // # of pin of Laser
int Detector = 7; // # of pin of Detector
void setup()
{
Serial.begin(9600);
Serial.print("Starting...\n");
pinMode(pwmA, OUTPUT);
pinMode(pwmB, OUTPUT);
pinMode(brakeA, OUTPUT);
pinMode(brakeB, OUTPUT);
digitalWrite(pwmA, HIGH);
digitalWrite(pwmB, HIGH);
digitalWrite(brakeA, LOW);
digitalWrite(brakeB, LOW);
pixy.init();
motor.setSpeed(80);
pinMode(Laser, OUTPUT);
pinMode(Detector, INPUT);
}
void loop()
{
//-----------------
// Laser sensor
//-----------------
digitalWrite(Laser, HIGH);
boolean val = digitalRead(Detector);
delay (500);
Serial.print(val); // 1 is detector, 0 is blocked
//------------------
// Eye Tracking
//------------------
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
blocks = pixy.getBlocks();
static int y1;
y1=0;
int x,y,y2,y3,y4,ypos1,ypos2,ypos3,ypos4,Detector;
int step1 = 100; //center position of pixy
if(val=1) // checking that laser is not at center
{
if (blocks)
{
i++;
for (j=0; j<blocks; j++)
{
if (pixy.blocks[j].height > 1)
{
if (pixy.blocks[j].y > 10 && pixy.blocks[j].y < 100)
{
y1 = pixy.blocks[j].y;
ypos1 = y_scale * (100 - y1);
motor.step(ypos1);
y3 = pixy.blocks[j].y;
if ( y3 > y1)
{
ypos3 = y_scale * (y3 - y1);
}
else if (y3 < y1)
{
ypos3 = y_scale * (y3 - y1);
}
}
else if (pixy.blocks[j].y > 100 && pixy.blocks[j].y < 180)
{
y2 = pixy.blocks[j].y;
ypos2 = y_scale * (100 - y2);
motor.step(ypos2);
y4 = pixy.blocks[j].y;
if ( y4 > y2)
{
ypos4 = y_scale * (y4 - y2);
}
else if (y4 < y2)
{
ypos4 = y_scale * (y4 - y2);
}
}
}
} // end of motor movement code
}
else if(val=1) // if the laser is at center
{
y1=0;y2=0;y3=0;y4=0;ypos1=0;ypos2=0;ypos3=0;ypos4=0; x=0;
}
}
}
eyetracking11.ino (2.74 KB)