Example #1
0
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);
}
Example #2
0
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;}
}
Example #4
0
// 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);
}
Example #5
0
/**
 * 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;}
Example #7
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;
}
Example #8
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;    
}
Example #10
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;
}
Example #11
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);
    }
}
Example #13
0
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;}