soccer.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
// set 1cm dist (ms)
// 1loop dist  = forwardDist(cm)*count (cm)

// 1 loop to goal = 10cm
// to catch ball = 3cm
#define forwardDist 3
//set 1 loop time
#define timeForward 145
#define countToGoal 4
#define countBallCatching 2

//below values must be changed before running
//thresholds
#define startThreshold 10
#define catchThreshold 10
//pos limits
#define xLimit 100
#define yLimit 100

//function prototypes
void posToVec();
void vecToPos();
void feint();
float getGyroHeading_t(tSensors gyro);

//global variables
float pos_x;
float pos_y;
float degree;
float distance;
float degree_t;
float distance_t;

task main(){
	int currentDistance = 0;
	int isDefend = 0;
	//set goal pos to (0,0) robot pos will be negative value

    // Create struct to hold sensor data
	tHTIRS2 irSeeker;

    // Initialise and configure struct and port
	initSensor(&irSeeker, S1);

	//Initialize Gyro and Robot pos data
	while(true){
		if(getXbuttonValue(xButtonEnter)){
			break;
		}
		currentDistance = SensorValue[sonar4];
		//update ultrasonic sensor value
		//display sensor value
		displayString(1,"Current Value:");
		displayString(2,"Distance:%3d cm",currentDistance);
	}

	//reset Gyro degrees
	resetGyro(gyro);


	//set start pos
	if(SensorValue[sonar4] < startThreshold){
	//if ultrasonic sensor value > Threshold then set pos data to offence
	    pos_x = 0;
		  pos_y = -90;
		  isDefend = 0;
	}else{
		//ultrasonic value < Threshold : Defense
		pos_x = 0;
		pos_y = -90;
		isDefend = 1;
	}
	degree = getGyroHeading_t(gyro);


	//offense
	if(isDefend == 0){
		repeatUntil(getGyroHeading_t(gyro) < 70){
			setMotorSpeed(rightMotor,-speedTurning);
			setMotorSpeed(leftMotor,speedTurning);
		}
		setMotorSpeed(leftMotor,speedForward);
		setMotorSpeed(rightMotor,speedForward);
		// 1 loop dist * 3
		wait1Msec(timeForward*countToGoal*3);

		//update robot vector
		degree = getGyroHeading_t(gyro);
		distance = countToGoal*forwardDist * 3;
		vecToPos();
	}


	//main control
	//loop condition;x != 0,y != 0 but pos value are set into floating value
	while(true){
		//read sensor values
		currentDistance = SensorValue[sonar4];
		degree = getGyroHeading_t(gyro);
		readSensor(&irSeeker);


		if(currentDistance < catchThreshold){
			//turn to goal
			posToVec();
			if((degree_t - degree) > 0){
				// target > current ; turn right
				setMotorSpeed(leftMotor,speedTurning);
				setMotorSpeed(rightMotor,-speedTurning);
				waitUntil(getGyroHeading_t(gyro) > degree_t);
			}else{
				//turn left
				setMotorSpeed(leftMotor,-speedTurning);
				setMotorSpeed(rightMotor,speedTurning);
				waitUntil(getGyroHeading_t(gyro) < degree_t);
			}

			//move forward: 10cm
			setMotorSpeed(leftMotor,speedForward);
			setMotorSpeed(rightMotor,speedForward);
			wait1Msec(timeForward*countToGoal);

			//update robot vector
			degree = getGyroHeading_t(gyro);
			distance = countToGoal*forwardDist;
			vecToPos();

			//feint; random 10%
			if((rand() % 100) > 90){
				feint();
			}

			displayTextLine(3,"goal");
		}else{
			//ball catch
			if(irSeeker.acDirection < 5){
				setMotorSpeed(leftMotor,-speedTurning);
				setMotorSpeed(rightMotor,speedTurning);
			}else if(irSeeker.acDirection > 5){
				setMotorSpeed(leftMotor,speedTurning);
				setMotorSpeed(rightMotor,-speedTurning);
			}else{
				setMotorSpeed(leftMotor,speedForward);
				setMotorSpeed(rightMotor,speedForward);
				wait1Msec(timeForward*countBallCatching);
				//update robot vector
				degree = getGyroHeading_t(gyro);
				distance = countBallCatching*forwardDist;
				vecToPos();
			}
			displayTextLine(3,"catch");
		}
			displayTextLine(1,"%f,%f",pos_x,pos_y);
			displayTextLine(2,"%f,%f",degree,distance);
			displayTextLine(4,"%f",degree_t);
	}

}

int ableFeint(){
	float slope_threshold = 90/60;
	float x,y;
	x = pos_x + 60;
	y = pos_y + 90;
	float slope;
	slope = y/x;
	if(slope < slope_threshold){
		return 1;
	}else{
		return 0;
	}
}

void feint(){
	int feint_deg = 20;
	float deg;

	if(ableFeint() == 1){
		deg = degree;
		if(rand() % 100 < 50){
			//turning right
			repeatUntil(getGyroHeading_t(gyro) > deg + feint_deg){
				setMotorSpeed(leftMotor,speedTurning);
				setMotorSpeed(rightMotor,-speedTurning);
			}
		}else{
			repeatUntil(getGyroHeading_t(gyro) < deg - feint_deg){
				setMotorSpeed(leftMotor,-speedTurning);
				setMotorSpeed(rightMotor,speedTurning);
			}
		}

		setMotorSpeed(leftMotor,speedForward);
		setMotorSpeed(rightMotor,speedForward);
		wait1Msec(timeForward*countToGoal);
		//set robot pos
		degree = getGyroHeading_t(gyro);
		distance = countToGoal*forwardDist;
		vecToPos();
	}
}

void vecToPos(){
	int switch_degree;
	float coef_x,coef_y;
	//set switch value
	switch_degree = floor(degree / 90.0 );

	switch(switch_degree){
		case 0:
			coef_x =  sinDegrees(degree);
			coef_y =  cosDegrees(degree);
			break;
		case 1:
			coef_x =  cosDegrees(degree - 90.0);
			coef_y = -sinDegrees(degree - 90.0);
			break;
		case 2:
			coef_x = -sinDegrees(degree - 180.0);
			coef_y = -cosDegrees(degree - 180.0);
			break;
		case 3:
			coef_x = -cosDegrees(degree - 270.0);
			coef_y =  sinDegrees(degree - 270.0);
			break;
	}
	pos_x = distance * coef_x + pos_x;
	pos_y = distance * coef_y + pos_y;
}

void posToVec(){
	float ab_x,ab_y;
	// set absolute pos value
	ab_x = fabs(pos_x);
	ab_y = fabs(pos_y);

	float deg;
	if(pos_x == 0){
		//if x value = 0 then deg will be 0
		degree_t = 0;
	}else{
		deg = radiansToDegrees(atan(ab_x/ab_y));
		if(pos_x < 0){
			//if x < 0 then robot must be turn right;
			degree_t =  deg;
		}else{
			degree_t = 360  - deg;
		}
	}

	distance_t = sqrt(pow(pos_x,2)+pow(pos_y,2));
	//return negative degree to x>0 positive degree to x<0
}

float getGyroHeading_t(tSensors gyro){
	float degree;
	degree = getGyroHeading(gyro);
	if(degree  < 0){
		degree = degree + 360;
	}

	return degree;
	}