void mouseClicks(int button, int state, int x, int y) {
if(button || state || x || y){}
if(!gameOver && !paused){
if(button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
shootGun();
}
glutPostRedisplay();
}
}
int main (void)
{
lidar.baud(115200);
pc.baud(115200);
servo.period(0.020);
on = 1;
j = 0;
points = 0;
xsum = 0;
ysum = 0;
xtarget = 0;
ytarget = 0;
xball = 0;
yball = 0;
ballFound = 0;
wait(1);
lidar.printf("testmode on\n");
pc.printf("testmode on\r\n");
wait(1);
lidar.printf("setldsrotation on\n");
pc.printf("setldsrotation on\r\n");
//Start the timer
timer.start();
while(1) {
if(on) {
if(timer.read_ms() >= 500) {
timer.reset();
getldsscan();
//Give the lidar some time to warm up before using data
if(j==11) {
//Find the walls so you don't crash into them
for (int i = 85; i<95; i++) {
wallSum = wallSum + x1[i];
}
wallL = wallSum/10;
pc.printf("wallL = %d\r\n", wallL);
wallSum = 0;
for (int i = 265; i<275; i++) {
wallSum = wallSum + x1[i];
}
wallR = wallSum/10;
pc.printf("wallR = %d\r\n", wallR);
wallSum = 0;
//Scan the range from 270 to 90 or front half of the robot for obstacles
for (int i=275; i<355; i++) {
if (x1[i]<(wallR-150) &&
x1[i]>(wallL+150) &&
abs(x1[i+1]-x1[i])>15 &&
abs(x1[i+1]-x1[i])<100 &&
x1[i]!=0) {
pc.printf("BALL: %d %d\r\n", x1[i], y1[i]);
xsum = xsum + x1[i];
ysum = ysum + y1[i];
points++;
}
}
for (int i=0; i<86; i++) {
if (x1[i]<(wallR-150) &&
x1[i]>(wallL+150) &&
abs(x1[i+1]-x1[i])>15 &&
abs(x1[i+1]-x1[i])<100 &&
x1[i]!=0) {
pc.printf("BALL: %d %d\r\n", x1[i], y1[i]);
xsum = xsum + x1[i];
ysum = ysum + y1[i];
points++;
}
}
xball = xsum/points;
yball = ysum/points;
pc.printf("Ball_1: %d %d\r\n", xball, yball);
xsum = 0;
ysum = 0;
points = 0;
//If there is a change in x or y then a ball is found else its an error
if (xball != 0 && yball != 0) {
ballFound = 1;
} else {
ballFound = 0;
}
//Move to the target location, notice that -200 is to account for length of robot
xtarget = xball;
ytarget = yball - 200;
//If the obstacle is too close a wall then don't move
if(xtarget < (wallL+150) || xtarget > (wallR + 150)) {
xtarget = 0;
ballFound = 0;
pc.printf("Near a wall.");
}
//If ball object is too close (touching the robot) then don't move
if(ytarget < 200) {
ytarget = 0;
ballFound = 0;
pc.printf("Object too close.");
}
if(ballFound == 1) {
//Send the motor command to the robot to move to the target in the y direction
currentAngle = 90;
shootGun();
wait(0.5);
lidar.printf("setMotor %d %d %d\n", (int)ytarget, (int)(ytarget*4/3), 250);
//Wait until still to start the lidar scan again
wait(ytarget/200);
//If xtarget is positive then 90 degree spin to the right
if (xtarget>0) {
lidar.printf("setMotor 140 -186 300\n");
wait(1.25);
//Move to the target in the x direction
lidar.printf("setMotor %d %d %d\n", (int)abs(xtarget)-200, (int)((abs(xtarget)-200)*4/3), 250);
wait(abs(xtarget/200));
//Spin back to start scanning again
lidar.printf("setMotor -140 186 300\n");
//Else the robot is to the left so 90 degree left spin
} else {
lidar.printf("setMotor -140 186 300\n");
wait(1.25);
lidar.printf("setMotor %d %d %d\n", (int)abs(xtarget)-200, (int)((abs(xtarget)-200)*4/3), 250);
wait(abs(xtarget/200));
lidar.printf("setMotor 140 -186 300\n");
}
timer.reset();
} else {
j = 10;
}
//Set the counter back to 5 so that there is time to remove obstacle or move person
j = 5;
led1 = 0;
ballFound = 0;
}
}
}
}
}
