soccer_new.c

#pragma config(StandardModel,"EV3_REMBOT")
#pragma config(Sensor,S2,gyro,sensorEV3_Gyro,modeEV3Gyro_RateAndAngle)
#pragma config(Sensor,S1,HTIRS2,sensorI2CCustom)
#pragma config(Sensor, S4,sonar4,sensorEV3_Ultrasonic)
// Change Sensor number
//connect Ultrasonic to S4


#include "\EV3\3rd party driver library\include\hitechnic-irseeker-v2.h"

#define speedForward  50
#define speedBackward  -25
#define speedTurning 15
//thresholds
#define startThreshold 10
#define catchThreshold 10
//
#define dist1Count 3
#define time1Count 145
#define countToGoal 3
#define countCatchBall 2

//function prototype
float posToVec();
void vecToPos(float degree,float distance);
void turnLeft();
void turnRight();
void moveForward();


//global variables
float pos_x;
float pos_y;

//must change return to task before run
task main(){

    int currentDistance =0;
    int isOffence = 0;
    float degree =0;
    float distance = 0;
    float target_degree = 0;

    //create irSeeker struct
    tHTIRS2 irSeeker;
    initSensor(&irSeeker,S1);

    while(true){
        if(getXbuttonValue(xButtonEnter)){
            break;
        }
        currentDistance = SensorValue[sonar4];
        displayString(1,"Current Value:");
        displayString(2,"Distance:%d cm",currentDistance);
    }

	//set Gyro degrees
	resetGyro(gyro);

    //set start pos
    if(SensorValue[sonar4] < startThreshold){
        //distance < threshold; defense
        pos_x = 0;
        pos_y = -90;
        isOffence = 0;
    }else{
        //distance > threshold; offence
        pos_x = 0;
        pos_y = -90;
        isOffence = 1;
    }

    //offence first move
    degree = getGyroHeading(gyro);

    if(isOffence == 1){
            //turn right and go strainght;
        repeatUntil(getGyroHeading(gyro) < 45){
            setMotorSpeed(rightMotor,-speedTurning);
            setMotorSpeed(leftMotor,speedTurning);
        }
        setMotorSpeed(leftMotor,speedForward);
        setMotorSpeed(rightMotor,speedForward);

        //move straight (goal count)*3 count;
        wait1Msec(time1Count*countToGoal*3);

        //update robot pos
        degree = getGyroHeading(gyro);
        distance = dist1Count*countToGoal*3;
        vecToPos(degree,distance);
    }

    //main control part
    while(true){
        //read sensor data
        currentDistance = SensorValue[sonar4];
        degree = getGyroHeading(gyro);
        readSensor(&irSeeker);

        if(currentDistance < catchThreshold){
            //robot-to-ball dist < catchThreshold;go to goal
            //must set 255 as 0

            //set robot toward goal
            target_degree = posToVec();
            if(target_degree > 0){
                if((target_degree -180 < getGyroHeading(gyro)) && (getGyroHeading(gyro) < target_degree)){
                    //t-180 < c and c < t; turn right
                    turnRight();
                    waitUntil(target_degree < getGyroHeading(gyro));
                }else if((target_degree < getGyroHeading(gyro)) || getGyroHeading(gyro) < target_degree - 180){
                    //t<c or c< t-180;turn left
                    turnLeft();
                    waitUntil((getGyroHeading(gyro) < target_degree) && (target_degree - 180 < getGyroHeading(gyro)));
                }
            }else{
                if((getGyroHeading(gyro) < target_degree) || (target_degree + 180 < getGyroHeading(gyro))){
                    //t>c or t+180 < c; turn right
                    turnRight();
                    waitUntil((target_degree < getGyroHeading(gyro)) && (getGyroHeading(gyro) < target_degree + 180));
                }else if((getGyroHeading(gyro) < target_degree) && (target_degree + 180 < getGyroHeading(gyro))){
                    //c < t and t+ 180 < c; turn left
                    turnLeft();
                    waitUntil(getGyroHeading(gyro) < target_degree);
                }
            }

            //move straight
            moveForward();
            wait1Msec(time1Count*countToGoal);

            //update robot pos
            degree = getGyroHeading(gyro);
            distance = dist1Count*countToGoal;
            vecToPos(degree,distance);


            displayTextLine(1,"%f,%f",pos_x,pos_y);
						displayTextLine(2,"%f,%f",degree,distance);
            displayTextLine(3,"goal");
        }else{
            //catch ball
			if(irSeeker.acDirection < 5){
					turnLeft();
			}else if(irSeeker.acDirection > 5){
					turnRight();
			}else{
				moveForward();
				wait1Msec(time1Count*countCatchBall);
				//update robot vector
				degree = getGyroHeading(gyro);
				distance = dist1Count*countCatchBall;
				vecToPos(degree,distance);
			}
        	displayTextLine(1,"%f,%f",pos_x,pos_y);
					displayTextLine(2,"%f,%f",degree,distance);
          displayTextLine(3,"catch");
        }
    }
}



float posToVec(){
//return goal-to-robot degrees from pos(global)
float ab_x,ab_y;

ab_x = fabs(pos_x);
ab_y = fabs(pos_y);

float deg;

deg = radiansToDegrees(atan(ab_x/ab_y));
if(pos_x < 0){
    //pos_x <0; degree var will be positive
    return deg;
}else{
    //pos_x =>0; degree var will be negative
    return -deg;
}

}

void vecToPos(float degree,float distance){
    float coef_x,coef_y;

    if(degree < 0){
        if(degree < -90){
            //degree < -90; 3rd quartant
            coef_x = -cosDegrees(fabs(degree) - 90);
            coef_y = -sinDegrees(fabs(degree) - 90);
        }else{
            //-90 < degree < 0; 4th quartant
            coef_x = -sinDegrees(fabs(degree));
            coef_y = cosDegrees(fabs(degree));
        }
    }else{
        if(degree < 90){
            //0 < degree < 90; 1st quartant
            coef_x = sinDegrees(degree);
            coef_y = cosDegrees(degree);
        }else{
            coef_x = cosDegrees(degree -90);
            coef_y = -sinDegrees(degree - 90);
        }
    }
    //update global var
    pos_x = distance *coef_x + pos_x;
    pos_y = distance * coef_y + pos_y;

}

void turnLeft(){
	setMotorSpeed(leftMotor,-speedTurning);
  setMotorSpeed(rightMotor,speedTurning);
}

void turnRight(){
	setMotorSpeed(leftMotor,speedTurning);
  setMotorSpeed(rightMotor,-speedTurning);
}

void moveForward(){
	setMotorSpeed(leftMotor,speedForward);
	setMotorSpeed(rightMotor,speedForward);
}