Skip to content

Instantly share code, notes, and snippets.

@kirlo-s
Created May 10, 2023 22:53
Show Gist options
  • Save kirlo-s/bcaee83dc307bac2d95e2d83e9e3df87 to your computer and use it in GitHub Desktop.
Save kirlo-s/bcaee83dc307bac2d95e2d83e9e3df87 to your computer and use it in GitHub Desktop.
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment