this is the code:
#include <CMUcam4.h>
#include <CMUcom4.h>
#include <Servo.h>
Servo sservo;
int motor = 10;
int ENDL = 22;
#define RED_MIN 0
#define RED_MAX 30
#define GREEN_MIN 0
#define GREEN_MAX 30
#define BLUE_MIN 0
#define BLUE_MAX 30
#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 5000 // 5 seconds
CMUcam4 cam(CMUCOM4_SERIAL3);
void setup()
{
cam.begin();
Serial.begin(9600);
cam.LEDOn(LED_BLINK); // Wait for auto gain and auto white balance to run.
delay(WAIT_TIME);
cam.lineMode(true);
cam.cameraBrightness(-120);
cam.cameraContrast(30);
cam.LEDOn(CMUCAM4_LED_ON);
sservo.attach(8);
sservo.write(90);
pinMode(motor,OUTPUT);
pinMode(ENDL,OUTPUT);
digitalWrite(ENDL,HIGH);
}
void loop()
{
int N=0;
float rulex1, rulex2, rulex3, rulex4, rulex5, rulex6, rulex7, rulex8, rulex9, rulex10, rulex11, rulex12, rulex13, rulex14, rulex15, rulex16, rulex17, rulex18, rulex19, rulex20, rulex21 =0;
float ruley1, ruley2, ruley3, ruley4, ruley5, ruley6, ruley7, ruley8, ruley9, ruley10, ruley11, ruley12, ruley13, ruley14, ruley15, ruley16, ruley17, ruley18, ruley19, ruley20, ruley21 =0;
float output1, output2, output3, output4, output5, output6, output7, output8, output9, output10, output11, output12, output13, output14, output15, output16, output17, output18, output19, output20, output21 =0;
float steering1, steering2, steering3, steering4, steering5, steering6, steering7, steering8, steering9, steering10, steering11, steering12, steering13, steering14, steering15, steering16, steering17, steering18, steering19, steering20, steering21 =0;
float speed1, speed2, speed3, speed4, speed5, speed6, speed7, speed8, speed9, speed10, speed11, speed12, speed13, speed14, speed15, speed16, speed17, speed18, speed19, speed20, speed21 =0;
float motor_speed;
int steering_angle;
float centroid_x=0;
float centroid_y=0;
float total_x=0;
float total_y=0;
float target_x=0;
float target_y=0;
CMUcam4_image_data_t f_data;
CMUcam4_tracking_data_t t_data;
cam.trackColor(RED_MIN, RED_MAX, GREEN_MIN, GREEN_MAX, BLUE_MIN, BLUE_MAX);
cam.getTypeTDataPacket(&t_data); // Get a tracking packet.
cam.getTypeFDataPacket(&f_data); // Get an image packet.
for (int y=14; y<49; y=y+2)
{
int blackcount=0;
int blacksum=0;
for (int x=9; x<70; x++)
{
if(cam.getPixel(&f_data,y,x)==1)
{
blackcount=blackcount+1;
blacksum=blacksum+x;
}
}
if (blackcount==0)
{
centroid_x=0;
centroid_y=0;
}
else
{
centroid_x=blacksum/blackcount;
centroid_y=y;
N=N+1;
}
total_x=total_x+centroid_x;
total_y=total_y+centroid_y;
}
if (N!=0)
{
target_x=total_x/N;
target_y=total_y/N;
Serial.print("targetx=");
Serial.print(target_x);
Serial.print(" targety=");
Serial.println(target_y);
if ((target_x>9.0&&target_x<=27.0)&&(target_y>0.0 &&target_y<=10.0)) // rule 1
{
if (target_x>9.0&&target_x<=18.0)
rulex1=target_x/(9.0)-1.0;
else if(target_x>18.0&&target_x<=27.0)
rulex1=target_x/(-9.0)+3.0;
else
rulex1=0.0;
if (target_y>0.0&&target_y<=5.0)
ruley1=target_y/(5.0);
else if(target_y>5.0&&target_y<=10.0)
ruley1=target_y/(-5.0)+2.0;
else
ruley1=0.0;
output1=min(rulex1,ruley1);
steering1=abs(output1)*77.0;
speed1=abs(output1)*150.0;
}
if ((target_x>9.0&&target_x<=27.0)&&(target_y>5.0&&target_y<=15.0)) // rule 2
{
if (target_x>9.0&&target_x<=18.0)
rulex2=target_x/(9.0)-1.0;
else if(target_x>18.0&&target_x<=27.0)
rulex2=target_x/(-9.0)+3.0;
else
rulex2=0.0;
if (target_y>5.0&&target_y<=10.0)
ruley2=1.0/5.0*target_y-1.0;
else if(target_y>10.0&&target_y<=15.0)
ruley2=(-1.0/5.0)*target_y+3.0;
else
ruley2=0.0;
output2=min(rulex2,ruley2);
steering2=abs(output2)*77.0;
speed2=abs(output2)*110.0;
}
if (target_x>9&&target_x<=27&&target_y>10&&target_y<=20) // rule 3
{
if (target_x>9.0&&target_x<=18.0)
rulex3=1.0/9.0*target_x-1.0;
else if(target_x>18.0&&target_x<=27.0)
rulex3=-1.0/9.0*target_x+3.0;
else
rulex3=0.0;
if (target_y>10.0&&target_y<=15.0)
ruley3=1.0/5.0*target_y-2.0;
else if(target_y>15.0&&target_y<=20.0)
ruley3=(-1.0/5.0)*target_y+4.0;
else
ruley3=0;
output3=min(rulex3,ruley3);
steering3=abs(output3)*60.0;
speed3=abs(output3)*110.0;
}
if ((target_x>9&&target_x<=27)&&(target_y>15&&target_y<=35)) // rule 4
{
if (target_x>9.0&&target_x<=18.0)
rulex4=1.0/9.0*target_x-1.0;
else if(target_x>18.0&&target_x<=27.0)
rulex4=-1.0/9.0*target_x+3.0;
else
rulex4=0.0;
if (target_y>15&&target_y<=25)
ruley4=1.0/10.0*target_y-1.5;
else if(target_y>25&&target_y<=35)
ruley4=(-1.0/10.0)*target_y+3.5;
else
ruley4=0.0;
output4=min(rulex4,ruley4);
steering4=abs(output4)*60;
speed4=abs(output4)*75;
}
if ((target_x>9&&target_x<=27)&&(target_y>25&&target_y<=59)) // rule 5
{
if (target_x>9.0&&target_x<=18)
rulex5=1.0/9.0*target_x-1.0;
else if(target_x>18.0&&target_x<=27.0)
rulex5=-1.0/9.0*target_x+3.0;
else
rulex5=0.0;
if (target_y>25.0&&target_y<=42)
ruley5=1.0/17.0*target_y-25.0/27.0;
else if(target_y>42.0&&target_y<=59)
ruley5=(-1.0/17.0)*target_y+59.0/17.0;
else
ruley5=0.0;
output5=min(rulex5,ruley5);
steering5=abs(output5)*60;
speed5=abs(output5)*75;
}
float total_output=output1+output2+output3+output4+output5+output6+output7+output8+output9+output10+output11+output12+output13+output14+output15+output16+output17+output18+output19+output20+output21;
steering_angle=(steering1+steering2+steering3+steering4+steering5+steering6+steering7+steering8+steering9+steering10+steering11+steering12+steering13+steering14+steering15+steering16+steering17+steering18+steering19+steering20+steering21)/total_output;
motor_speed=(speed1+speed2+speed3+speed4+speed5+speed6+speed7+speed8+speed9+speed10+speed11+speed12+speed13+speed14+speed15+speed16+speed17+speed18+speed19+speed20+speed21)/total_output;
Serial.print(steering_angle);
Serial.print(" ");
Serial.println(motor_speed);
sservo.write(steering_angle);
analogWrite(motor,motor_speed);
}
else
{
sservo.write(90);
analogWrite(motor,0);
}
Serial.print(" steering angle=");
Serial.print(steering_angle);
Serial.print(" running speed=");
Serial.println(motor_speed);
}
// Do something else here.