void serverConnect(){
connect_to_server("130.195.6.196", 1024);
send_to_server("Please");
char message[24];
receive_from_server(message);
send_to_server(message);
}
int main(){
init(1);
int x;
connect_to_server("130.195.6.196", 1024); //Opening the gate code
send_to_server("Please");
char message[24];
receive_from_server(message);
printf("%s", message);
send_to_server(message);
for (x = 0; x < 8; x++)
{
select_IO(x,0);
write_digital(x,1);
}
while(1){
take_picture();
int sum = 0;
float kp = 0.5;
int w;
int s;
int proportional_signal=0;
for(int x = 0; x < 320; x++){
w = get_pixel(x,120,3);
if(w>127){s=1;}
else{s=0;};
sum = sum + (x-160)*s;}
proportional_signal = sum * kp;
if (sum < 0){
set_motor(1, proportional_signal);
set_motor(2, -1*proportional_signal);
}
else if (sum > 0){
set_motor(1, -1*proportional_signal);
set_motor(2, proportional_signal);
}
else{
set_motor(1, proportional_signal);
set_motor(2, proportional_signal);
}
update_screen();
for (x = 0 ; x < 8; x++)
{
int adc_reading = read_analog(x);
printf("ai=%d av=%d\n",x,adc_reading);
}
}
set_motor(1,0);
set_motor(2,0);
return 0;
}
int openDoor(){
if(connect_to_server("130.195.6.196", 1024) == 0){
send_to_server("Please");
char message[24];
receive_from_server(message);
send_to_server(message);
return 0;
}else{return 1;}
}
// https://github.com/kaiwhata/ENGR101-2016/wiki/Network-commands
// more detail on open gate
void openGate(){
connect_to_server((char *)"130.195.6.196", 1024);
send_to_server((char *)"Please");
char message[24];
receive_from_server(message);
// null character limits message length at 6 characters long
message[MESSAGE_LENGTH] = '\0';
send_to_server(message);
printf("%s", message);
}
/**
* Sends a request to the gate server to open the gate.
*/
void open_gate()
{
// Establishes a connection to the gate's server
connect_to_server(ip, port);
// Sends the request to open the gate
send_to_server(request);
// Receives the password from the server
receive_from_server(password);
// Sends the password for the gate
send_to_server(password);
}
int main (){
// This sets up the RPi hardware and ensures
// everything is working correctly
init(1);
//connects to server with the ip address 192.168.1.2
connect_to_server("130.195.6.196", 1024);
//sends a message to the connected server
send_to_server("Please");
//receives message from the connected server
char message[24];
receive_from_server(message);
send_to_server(message);
printf("%s", message);
return 0;}
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.02;
float kd = 0.04;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
int leftSensor, rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250; //Sensor Threshold
int totalWidth;
//Primary Initialization
init(1);
//Networking Section
//Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 60, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 180, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 5){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, 50);
Sleep(0, 100000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
// set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
set_motor(2, -((int) (((whiteLocation*40/120)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
}
return 0;
}
int main()
{
init(0);
signal(SIGINT, terminate);
signal(SIGTERM, terminate);
if (gate_test == 1) {
//connects to server with the ip address 130.195.6.196
connect_to_server("130.195.6.196", 1024);
//sends a message to the connected server
send_to_server("Please"); // "Please is the 'codeword', may change
//receives message from the connected server
char message[24];
receive_from_server(message);
send_to_server(message); // send password back to server to open up gate.
}
Sleep(1, 000000);
speed = 90;
v_left = speed - Tcorrection; // speed of left motor
v_right = speed + Tcorrection; // speed of right motor
// Method for completing quadrant 1
while (method == 1) {
Dconstant = 340;
clock_gettime(CLOCK_REALTIME, &now);
prev_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
take_picture();
n_whites_mid = 0;
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
midPic[i] = get_pixel(x, 1, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
}
// reset variables
error_mid = 0.0;
for (int l = 0; l < NTP; l++) {
error_mid = error_mid + (zeroCentre[l] * midPic[l]);
}
if (n_whites_mid != 0 && n_whites_mid != NTP) {
GeneralPID();
}
else if (n_whites_mid == NTP) {
method = 2;
}
else if (n_whites_mid == 0) {
// if loses line, swings quickly to direction of last error of line seen
if (Perror_mid > 0) {
// hard left
v_left = -15;
v_right = 40;
}
else if (Perror_mid < 0) {
// hard right
v_left = 40;
v_right = -15;
}
}
Perror_mid = error_mid;
clock_gettime(CLOCK_REALTIME, &now);
now_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
delta_ms = now_ms - prev_ms;
set_motor(1, v_left);
set_motor(2, v_right);
}
// method for completing quadrant 3
while (method == 2) {
speed = 65;
Pconstant = 0.6;
Dconstant = 375;
clock_gettime(CLOCK_REALTIME, &now);
prev_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
take_picture();
n_whites_mid = 0;
n_whites_left = 0;
n_whites_right = 0;
sensor_right = read_analog(2);
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
int y = dy * i + (dy / 2);
midPic[i] = get_pixel(x, 1, 3);
leftPic[i] = get_pixel(1, y, 3);
rightPic[i] = get_pixel(319, y, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
if (leftPic[i] > THRESHOLD) {
// 1 = white
leftPic[i] = 1;
n_whites_left++;
}
else {
// 0 = black
leftPic[i] = 0;
}
if (rightPic[i] > THRESHOLD) {
// 1 = white
rightPic[i] = 1;
n_whites_right++;
}
else {
// 0 = black
rightPic[i] = 0;
}
if (sensor_right > 300) {
method = 3;
}
}
// reset variables
error_mid = 0.0;
for (int l = 0; l < NTP; l++) {
error_mid = error_mid + zeroCentre[l] * midPic[l];
}
// t junction
if (midPic[16] == 0 && n_whites_left != 0 && n_whites_right != 0) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process(); // calls method to take image and process values
v_left = speed;
v_right = -0.7 * speed;
set_motor(2, v_right);
set_motor(1, v_left);
}
}
// left turn
else if (midPic[16] == 0 && n_whites_left != 0 && n_whites_right < 5) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process();
v_left = speed;
v_right = -0.7 * speed;
set_motor(2, v_right);
set_motor(1, v_left);
}
}
//right turn
else if (midPic[16] == 0 && n_whites_left < 5 && n_whites_right != 0) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process();
v_left = speed;
v_right = -0.7 * speed;
set_motor(1, v_right);
set_motor(2, v_left);
}
}
// dead end - should do 180
else if (n_whites_mid == 0 && n_whites_left == 0 && n_whites_right == 0) {
while (midPic[16] == 0) {
Quad3Process();
v_left = -speed;
v_right = speed;
set_motor(1, v_right);
set_motor(2, v_left);
}
}
// pid
else if (n_whites_mid != 0 && n_whites_mid != NTP) {
GeneralPID();
}
else if (n_whites_mid == 0) {
v_left = 45;
v_right = 45;
}
Perror_mid = error_mid;
clock_gettime(CLOCK_REALTIME, &now);
now_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
delta_ms = now_ms - prev_ms;
set_motor(1, v_left);
set_motor(2, v_right);
}
while (method == 3) {
Pconstant = 0.6;
int error;
Perror_mid = error;
speed = 40;
Dconstant = 0.35;
// Loop here:
sensor_left = read_analog(0);
sensor_mid = read_analog(1);
sensor_right = read_analog(2);
error = sensor_right - sensor_left;
proportional = error * Pconstant;
proportional = ((proportional / 770) * 100);
derivative = ((error - Perror_mid) / 0.1) * Dconstant;
if (sensor_mid > 470) {
set_motor(1, -speed);
set_motor(2, speed);
Sleep(0, 350000);
}
set_motor(2, speed - proportional);
set_motor(1, speed + proportional);
}
}
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.0;
float kd = 0.0;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
//int motorOne, motorTwo;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
signed int leftSensor;
signed int rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250;
int totalWidth;
signed int leftMotor, rightMotor;
//Primary Initialization
init(1);
//Networking Section
// Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("Please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
set_motor(1, 43);
set_motor(2, -40);
Sleep (5,0);
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 10, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 230, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 3){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 300000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// motorOne = (-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))
// motorTwo = (((whiteLocation*1/3)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)
// set_motor(1, motorOne);
// set_motor(2, -motorTwo);
//set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
//set_motor(2, -((int) (((whiteLocation*1/3)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
whiteWall = 0;
//returns true if there isnt a wall
noLeftWall =false;
noRightWall = false;
noWallAhead = false;
//get data from sensors
leftSensor = read_analog(0);
rightSensor = read_analog(1);
//printf("left sensor: %d\nright sensor: %d\n", leftSensor, rightSensor);
//get data from camera
take_picture();
for(int i = 120; i<128; i++){
c = get_pixel(300,i, 3);
if(c>120){ //change white threshold
whiteWall++;
}
}
printf("whiteWall: %d\n", whiteWall);
if(whiteWall < 5){ //Change threshold if theres problems
noWallAhead = true; //rename
// printf("No wall ahead!!!!!!!!!\n\n\n");
}
if(leftSensor<THRESHOLD){
noLeftWall = true;
}
if(rightSensor<THRESHOLD){
noRightWall = true;
}
if(noRightWall){
set_motor(1, 32);
set_motor(2, -30);
Sleep(0, 300000);
set_motor(1, 37);//right motor
set_motor(2, -67);//left motor//CHANGE THRESHOLD
Sleep(0,900000);//CHANGE THRESHOLD
printf("turning right\n");
}
else if(noWallAhead){ //stay in the center of the maze
rightMotor = (leftSensor/10*1.1); //change threshold
leftMotor = -(rightSensor/10);
set_motor(1, rightMotor);
set_motor(2,leftMotor);
Sleep(0,1);
//rotate back to centre
leftMotor = -(leftSensor/10); //change threshold
rightMotor = (rightSensor/10*1.1);
set_motor(1, rightMotor);
set_motor(2, leftMotor);
printf("forwarsdgsdjksdgr\n");
Sleep(0, 1);
}
else if(noLeftWall){
set_motor(1, 40);
set_motor(2,-42);
Sleep(0,450000);
set_motor(1, 66);//right motor
set_motor(2, -32);//left motor //Change thresholds
Sleep(0,900000);
printf("left left left left\n");
}
// else //pop a u turn
/* {
printf("pop a u turn\n");
set_motor(1, -50);
set_motor(2, -60); //bigger so the back doesn't hit the wall
Sleep(0,100000); //Change thresholds
}*/
}
return 0;
}
int main (){
init_hardware();
// init(0);
int brightnessArray[WIDTH];
int threshold = 120;
int generatedArray[WIDTH];
int errorArray[WIDTH];
int lagErrorArray[WIDTH];
int baseSpeed = 35;
int leftSpeed=baseSpeed;
int rightSpeed=baseSpeed;
bool check=true;
int countrev=0;
int previousError = 0;
int derivative = 0.0;
double kd = 0.00000005;
bool checkintersection=false;
int intcount=0;
int sumError=0;
int incount=0;
bool right90=true;
bool left90=true;
int stuckCount = 0;
//network
bool gate = true;
char ip[14] = "130.195.6.196";
char message[7];
int ir_sensor = read_analog(0);
//connect_to_server(ip,1024);
double k=0.0008; // constant
// an array starting from -160 to 160
int number = -160;
for(int y=0; y<WIDTH+1; y++){
generatedArray[y] = number;
number++;
}
int counter = 0;
int zeroCount = 0;
while(true){
take_picture();
ir_sensor=read_analog(0);
//display_picture(0.5,0);
//open_screen_stream();
//update_screen();
previousError = sumError;
sumError=0;
for(int i=0; i<WIDTH; i++){
char colour = get_pixel(i, HEIGHT/2, 3);
if(colour > threshold){
brightnessArray[i] = 1;
} else {
brightnessArray[i] = 0;
}
errorArray[i] = brightnessArray[i] * generatedArray[i];
sumError += errorArray[i];
}
derivative = ((sumError-previousError)/0.000025);//.000025
printf("Sum error: %d\n",sumError);
printf("Previous error: %d\n",previousError);
//90 DEGREE TURNS
//Right and left 90 Degree turn
//int nine=0;
/*
int turnCounter = 0;
if(sumError>10000){ //sum should be large as the right hand side(0-160) will be white
right90=true;
}else{
right90=false;
}
if(sumError<-10000){ //sum should be very small as the left hand side(0 to -160) will be white
left90=true;
}else{
left90=false;
}
if(right90){
while(turnCounter<25){
set_motor(0,baseSpeed);
set_motor(1,-baseSpeed);
turnCounter++;
}
}
if(left90){
while(sumError<25){
set_motor(0,-baseSpeed);
set_motor(1,baseSpeed);
turnCounter++;
}
}
turnCounter=0;
*/
// calculate speed for left and right motors k has to be small like 0.1
//INTERSECTION CODE
//for(int in=0;in<WIDTh;in++){
//if((birghtnessArray[h])==1){ //Goes through array and because the camera should read all white the sum should have very few 0's
//checkintersection=true; //if it counts 4 blacks then its not an intersection
//}
//else if{intcount<4){
//checkintersection=true;
//incount++
//}
//else if(intcount==4){
//checkintersection=false;
//break;
//}
//}
//if(checkintersection){
//set_motor(0,45)
//set_motor(1,45)
//}
//}//~~~~~unsure what this is for/what it closes off
for(int h=0;h<WIDTH;h++){
if((brightnessArray[h])==0){
check=false;
}else if(countrev<4){
check=false;//this used to be true, changed it to false in case of the last error vlaue.
countrev++;
}else if (countrev==4){
check=true;
break;
}/*
if ((brightnessArray[h])==1){
checkintersection=true;
}else if(intcount<4){
checkintersection=true;
incount++;
}else if(intcount==4){
checkintersection=false;
break;
}*/
//continue;
}
/*for(int i=0; i<WIDTH;i++){
if(brightnessArray[i]==1){
checkintersection=true;
}else if(intcount<1){
checkintersection = true;
incount++;
}else if(intcount==1){
checkintersection=false;
break;
}
}*/
if(check==true){ // && checkintersection==false){
// printf("Kd*d: %i\n",(kd*derivative));
leftSpeed = baseSpeed + (int)(((k*sumError)-(kd*derivative))); // sumError should be 0 for the robot to go in a straight line
rightSpeed = baseSpeed - (int)(((k*sumError)+(kd*derivative)));
printf("K*sum: %d\n",(k*sumError));
printf("Derivative: %d\n",derivative);
/* if(leftSpeed<baseSpeed){
leftSpeed = 0
} else if(rightSpeed<baseSpeed){
rightSpeed = 0;
}
*/
if(leftSpeed > 255){
leftSpeed = 255;
}
else if(leftSpeed < -255){
leftSpeed = -255;
}
if(rightSpeed > 255){
rightSpeed = 255;
} else if(leftSpeed < -255){
rightSpeed = -255;
}/*
if(sumError>6000 || sumError<-6000){
set_motor(1, baseSpeed);
set_motor(2, baseSpeed);
}*/
if(sumError==0 && previousError<0){
leftSpeed = 0;
rightSpeed = baseSpeed;
}
else if(sumError==0 && previousError>0){
leftSpeed = baseSpeed;
rightSpeed = 0;
}/*else if(sumError==0 && previousError==0){
set_motor(1, baseSpeed);
set_motor(2,-baseSpeed);
}*/
else {
set_motor(1, leftSpeed); // Motor 1 is left motor//left
set_motor(2, rightSpeed);//right
}
printf("Left Speed = %d, Right Speed =%d\n", leftSpeed, rightSpeed);
printf("Sum error: %d\n",sumError);
Sleep(0, 25000); // sleeps for 50 ms
counter++;
if(counter==1000){
set_motor(1, leftSpeed); // Motor 1 is left motor
set_motor(2, rightSpeed);
}
} else if(check==false){// && checkintersection==false) {
//int countback=0;
//while(countback<4){
set_motor(1,-baseSpeed);
set_motor(2,-baseSpeed);
//Sleep(0,5000000);
//countback++;
//}
}/* else if(check==true){
set_motor(1,baseSpeed);
set_motor(2,baseSpeed);
Sleep(3,0);
set_motor(1,0);
set_motor(2, baseSpeed);
}*/
if(gate==true && ir_sensor>200){
// printf("%d", ir_sensor);
set_motor(1, 0);
set_motor(2, 0);
connect_to_server(ip, 1024);
//if(ir_sensor > 1){
send_to_server("Please");
receive_from_server(message);
send_to_server(message);
//gate = true;
//}
gate=false;
Sleep(2,0);
//might be buggy and need to double check
//printf("%s", message);
}
}
return 0;
}
/*
* asynchronus input (select) with the stdin and the socket connection
* to the server. copy input data from stdin to server, and
* copy received data from the server to stdout/stderr.
* destination of output is stdout or stderr are distinguished by
* stream identifier(sid) header is 'out' or 'err'.
*/
int start_session(int fd)
{
struct header_t headers[MAX_HEADER];
#ifdef WINDOWS
invoke_thread(fd);
#else
fd_set read_set;
int ret;
int stdin_closed = 0;
#endif
while (1) {
#ifdef UNIX
// initialize the set of file descriptor
FD_ZERO(&read_set);
// watch stdin of client and socket.
FD_SET(STDIN_FILENO, &read_set);
FD_SET(fd, &read_set);
if ((ret = select(fd + 1, &read_set, (fd_set*)NULL, (fd_set*)NULL, NULL)) == -1) {
perror("ERROR: select failure");
exit(1);
}
if (ret == 0) {
fprintf(stderr, "ERROR: timeout?\n");
continue;
}
// detect changed descriptor
if (!stdin_closed && FD_ISSET(STDIN_FILENO, &read_set)) { // stdin
#ifdef DEBUG
fprintf(stderr, "DEBUG: detect stdin\n");
#endif
stdin_closed = send_to_server(fd);
continue;
}
if (FD_ISSET(fd, &read_set) == FALSE){ // socket
#ifdef DEBUG
fprintf(stderr, "DEBUG: detect nothing\n");
#endif
continue;
}
#ifdef DEBUG
fprintf(stderr, "DEBUG: detect socket\n");
#endif
#endif
int size = read_headers(fd, headers);
if (size == 0) {
return 0; // as normal exit if header size 0
}
// Process exit
char* status = find_header(headers, HEADER_KEY_STATUS, size);
if (status != NULL) {
int stat = atoi(status);
return stat;
}
// Dispatch data from server to stdout/err.
char* sid = find_header(headers, HEADER_KEY_CHANNEL, size);
if (sid == NULL) {
fprintf(stderr, "ERROR: required header %s not found\n", HEADER_KEY_CHANNEL);
return 1;
}
char* chunk_size = find_header(headers, HEADER_KEY_SIZE, size);
if (chunk_size == NULL) {
fprintf(stderr, "ERROR: required header %s not found\n", HEADER_KEY_SIZE);
return 1;
}
if (receive_from_server(fd, sid, atoi(chunk_size)) == EOF) {
return 0;
}
}
}
int main() {
init(0);
//Networking Code
connect_to_server("130.195.6.196",1024);
send_to_server("Please");
char message[24];
receive_from_server(message);
send_to_server(message);
//Signal Catcher code
signal(2, signal_callback_handler);
//Declaration of Constants for 2nd Quadrant
double errorSum = 0;
double errorSum2 = 0;
double kp = 0.5;
double kd = 5;
double proportional_signal;
open_screen_stream();
int quad_three = 0;
printf("QUADRANT ONE AND TWO\n");
while(1) {
quad_three = 0;
take_picture();
//CODE FOR PROPORTIONAL
for (int i=0; i < 320; i++){
if(get_pixel(i, 160, 3) > THRESH) {
errorSum += (i-160);
quad_three++;
}
}
//CODE FOR FUTURE LINE ERROR
for (int i=0; i < 320; i++){
if(get_pixel(i, 80, 3) > THRESH) {
errorSum2 += (i-160);
}
}
errorSum/=160;
errorSum2/=160;
//THIS IS NOT ACTUALLY DERIVATIVE SIGNAL - THIS CODE:
//Looks to see if there any curves in the line and slows down based on how curvy the curve is
int derivative_signal = abs(errorSum-errorSum2)*kd;
if(derivative_signal > 33) {
derivative_signal = 33;
}
proportional_signal = errorSum * kp;
int leftSpeed = BASE_SPEED + proportional_signal - derivative_signal;
int rightSpeed = BASE_SPEED - proportional_signal - derivative_signal;
if (proportional_signal < - 0.5 && proportional_signal > -0.6 && quad_three != 320){
for(int i = 0; i < 10; i++) {
move(BASE_BACK_SPEED, BASE_BACK_SPEED);
}
} else {
if(quad_three == 320)break;
move(leftSpeed, rightSpeed);
}
}
printf("THIRD QUADRANT\n");
move(50,50);
Sleep(0,200000);
while(1) {
if (p(160,0.3,50,0) > 310 && p(40,0.3,50,0) < 80) {
turn(1);
break;
}
}
int red = 0;
int blue = 0;
int green = 0;
while(red < 200 || green > 100 || blue > 100) {
take_picture();
red = get_pixel(120,160,0);
green = get_pixel(120,160,1);
blue = get_pixel(120,160,2);
int p160 = p(160,0.7,40,0);
int p80 = p(80,0.7,40,0);
if (p160 > 310 && p80 < 80) {
turn(1);
}
}
move(70,70);
Sleep(0,200000);
int same_count = 0;
double prev_signal = 0;
int count = 0;
int prevTotal = 0;
while (1){
int left_signal = read_analog(1);
int right_signal = read_analog(0);
int base_speed = 45;
double constant = 0.075;
/*printf("Left %d\n",left_signal);
printf("Right %d\n",right_signal);*/
double actual_signal = left_signal - right_signal;
actual_signal *= constant;
actual_signal += 4.8;
printf("Actual %f\n",actual_signal);
if (actual_signal > 20 || actual_signal < -20){
base_speed = 35;
} else {
base_speed = 45;
}
move(base_speed + actual_signal, (base_speed - actual_signal)*1.3);
}
}
int main(){
init(1);
signal(SIGTERM, terminate);
signal(SIGINT, terminate);
connect_to_server("130.195.6.196", 1024);
send_to_server("Please");
char message[24];
receive_from_server(message);
printf("%s", message);
send_to_server(message);
while(1){
take_picture();
int pixelval = 0;
int num = 0;
int error=0;
int s;
int leftpix=0;
int centerpix=0;
int rightpix=0;
int forwardpix=0;
int numL=0;
int numR=0;
int numC=0;
int numF=0;
for(int x = 0; x <20 ; x++){ //LEft pixels 0 - 10
pixelval = get_pixel(x,120,3);
if(pixelval > 95){
// pixel white
numL = 1;
}
else{
//pixel black
numL = 0;
}
leftpix = leftpix + numL;
}
for(int x = 155; x <165 ; x++){ //Center pixels 155-165
pixelval = get_pixel(x,120,3);
if(pixelval > 95){
// pixel white
numC = 1;
}
else{
//pixel black
numC = 0;
}
centerpix = centerpix + numC;
}
for(int x = 300; x <320 ; x++){ //Right pixels 310-320
pixelval = get_pixel(x,120,3);
if(pixelval > 95){
// pixel white
numR = 1;
}
else{
//pixel black
numR = 0;
}
rightpix = rightpix + numR;
}
for(int x = 0; x <30 ; x++){ //Going Downn camera Y
pixelval = get_pixel(140,x,3); //To check if there is a line ahead
if(pixelval > 95){
// pixel white
numF = 1;
}
else{
//pixel black
numF = 0;
}
forwardpix = forwardpix + numF;
}
printf("num=%d\n", num);
printf("center=%d\n", centerpix);
printf("error=%d\n",error);
printf("forward=%d\n",forwardpix);
printf("Left=%d\n",leftpix);
printf("Right=%d\n",rightpix);
if(centerpix>7 && leftpix>10 && rightpix>10 && forwardpix>=30){ //if there's a intersection, make the robot move backwards
printf("Intersection++++++++++++++++++++++++++++++++++\n");
set_motor(1,-200);
set_motor(2,-200);
Sleep(0,500000);
}else if(centerpix>7 && leftpix>10 && rightpix>10 && forwardpix<30){
printf("TInter===================================\n");
set_motor(1,-125);
set_motor(2,-125);
Sleep(0,400000);
set_motor(1,-178);
set_motor(2,178);
Sleep(1,0000);
}else if(centerpix>7 && leftpix>7 && rightpix==0 && forwardpix<20){
printf("left\n");
set_motor(1,-125);
set_motor(2,-125);
Sleep(0,500000);
set_motor(1,-177);
set_motor(2,177);
Sleep(1,0000);
}else if(centerpix>7 && rightpix>10 && leftpix==0 && forwardpix<20){
printf("right\n");
set_motor(1,-125);
set_motor(2,-125);
Sleep(0,500000);
set_motor(1,177);
set_motor(2,-177);
Sleep(1,00000);
}else{
for(int x = 0; x < 320; x++){
pixelval = get_pixel(x,120,3);
if(pixelval > 127){
// pixel white
s = 1;
}
else{
//oixel black
s = 0;
}
error = error+(x-160)*s;
num = num + s;
}
if(num>0){
// at least one white pixel
printf("Forward\n");
error=error/num;
//printf("%d\n", pixelval);
//error=error/num;
int v1 = -90 + 0.86*error;
int v2 = -90 - 0.86*error;
printf("v1 = %d v2 = %d\n",v1,v2);
set_motor(1,v1);
set_motor(2,v2);
}else{
// no white pixels at all
printf("Backing\n");
set_motor(1,70);
set_motor(2,75);
}
Sleep(0,500000);
}
printf("////////////////// \n");
}// while
return 0;}
