 | Code: #pragma config(Sensor, S1, , sensorI2CCustom) #pragma config(Sensor, S4, sonar, sensorSONAR) #pragma config(Motor, motorA, frontLeftMotor, tmotorEV3_Large, PIDControl,reversed, encoder) #pragma config(Motor, motorB, frontRightMotor, tmotorEV3_Large, PIDControl, reversed, encoder) #pragma config(Motor, motorC, backLeftMotor, tmotorEV3_Large, PIDControl, reversed, encoder) #pragma config(Motor, motorD, backRightMotor, tmotorEV3_Large, PIDControl, reversed, encoder) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//Robot:Grasshopper Vers. 1.0 EV3, by Sigtrygg (2015)
#include "drivers/hitechnic-compass.h" //take care for choosing the right path!
//Variables const int WHEELCIRCUMSTANCE=50;//cm //const int TRACKWIDTH=22;//cm const float PI=3.14159265; float rpmSetPoint [4]={0,1,2,3}; float rpmMotorNow [4]={0,1,2,3}; int rpmCounter=0; int motorNumber=0; const int rpm_threshold = 10; // An RPM below 10% the expected RPM value at a specific speed will be considered a stall const int rpm_large = 175; // 175 This is the official RPM an unloaded large EV3/NXT motor has float rpmComparisionValue=0;
// Create struct to hold sensor data tHTMC compass;
//local data for calculations float lon1; float lat1;
//destination data for calculations float lon2; float lat2;
float distance;//distance to destination short targetangle;//angle to destination
int max_velocity=55; int Velocity=0; float curvefactor_ahead=1.3; //higher value => smaller curves, lower value => larger curves
float dx,dy; int sonarvalue=0;
//Declare functions void turnLeft(int Velocity,float curvefactor, long ratio); void turnRight(int Velocity,float curvefactor, long ratio); void driveBack(int Velocity, int distance); void Positioning(); void avoidObstacle (); void setRPMTargetToZero(); void readCompass_and_show(); void stopAllMotors(); void checkMotorStall();
float calculate_distance() { float l_factor=(float)111300*cos(((lat1+lat2)/2)*PI/180); dx=l_factor*(float)(lon1-lon2); dy=(float)111300*(lat1-lat2); distance=sqrt(dx*dx+dy*dy); return distance; //in meters !!! }
float calculate_targetangle()//angle for destination { if (lon2>lon1&&lat2>lat1) targetangle=90-atan((dy/dx))*180/PI;//acos(distance_L/distance)*180/PI; if(lon2>lon1&&lat1>lat2) targetangle=180-(90+atan(dy/dx)*180/PI);//(distance_L/distance)*180/PI+90; if (lon1>lon2&&lat1>lat2) targetangle=270-atan(dy/dx)*180/PI;//(distance_L/distance)*180/PI+180; if (lon1>lon2&&lat2>lat1) targetangle=360-(90+atan(dy/dx)*180/PI);//(distance_L/distance)*180/PI+270; return targetangle; }
void avoidObstacle () //drive back for 50cm and turn left for 3 seconds { driveBack(30,50); //speed of 30% and distance of 50 cm clearTimer (timer1); while (time1[T1] <2000) { turnLeft(40, 1.6, 25); //speed of 40% and curvefactor of 1.6 } stopAllMotors(); }
//======================== DRIVING FUNCTIONS =======================================
//With this function the robot drives ahead to the waypoint in curves. //It compares continuously its currently heading with the target heading and corrects its heading, if necessary.
void driveAhead() { playSoundFile("Forward");
float Encoderdegrees=(float)(distance*100*360)/WHEELCIRCUMSTANCE; //*100 because of meters!! resetMotorEncoder(backLeftMotor);
clearTimer(T2);
rpmCounter=0;
while (nMotorEncoder[backLeftMotor] < Encoderdegrees) { readCompass_and_show(); //read compass values every 200msec
if (time1[T2]<2000) { Velocity=max_velocity/2; } if (time1[T2]>=2000) { Velocity=max_velocity; } if (compass.relativeHeading>0)//turn left { turnLeft(Velocity, curvefactor_ahead, 15); } if (compass.relativeHeading<0) //turn right { turnRight(Velocity, curvefactor_ahead,15); }
if (compass.relativeHeading==0) //straight ahead { setMotorSync(frontLeftMotor, frontRightMotor, 0, -Velocity); setMotorSpeed(backRightMotor, Velocity); setMotorSpeed(backLeftMotor, Velocity); rpmSetPoint [0]=Velocity*rpm_large/100; rpmSetPoint [1]=Velocity*rpm_large/100; rpmSetPoint [2]=Velocity*rpm_large/100; rpmSetPoint [3]=Velocity*rpm_large/100; }
//monitoring stall of motors checkMotorStall();
//check for obstacles!! sonarvalue = SensorValue[sonar]; if (sonarvalue<30 && sonarvalue>0)//>0 because of avoiding inital error { playSoundFile("Ouch"); avoidObstacle (); wait1Msec(250); } } setRPMTargetToZero(); //Reset stall detection
stopAllMotors();
wait1Msec(500); }
void driveBack(int Velocity, int distance)//distance in cm { float Encoderdegrees=(float)(distance)*360/WHEELCIRCUMSTANCE; setMotorSyncEncoder(backLeftMotor, backRightMotor, 0, Encoderdegrees, Velocity); moveMotorTarget(frontLeftMotor, -Encoderdegrees, -Velocity); moveMotorTarget(frontRightMotor, -Encoderdegrees, -Velocity); waitUntilMotorStop(backLeftMotor); }
void turnRight(int Velocity, float curvefactor, long ratio) { setMotorSync(frontLeftMotor,frontRightMotor, ratio, -Velocity); setMotorSpeed(backLeftMotor, Velocity); setMotorSpeed(backRightMotor, Velocity/(curvefactor*1.1)); //1.1 = correction because of different track width of front and back wheels rpmSetPoint [0]=Velocity*rpm_large/100; rpmSetPoint [1]=(Velocity/curvefactor)*rpm_large/100; rpmSetPoint [2]=Velocity*rpm_large/100; rpmSetPoint [3]=(Velocity/(curvefactor*1.1))*rpm_large/100;
}
void turnLeft(int Velocity, float curvefactor, long ratio) { setMotorSync(frontRightMotor, frontLeftMotor, -ratio, -Velocity); setMotorSpeed(backRightMotor, Velocity); setMotorSpeed(backLeftMotor, Velocity/(curvefactor*1.1)); //1.1 = correction rpmSetPoint [0]=(Velocity/curvefactor)*rpm_large/100; rpmSetPoint [1]=Velocity*rpm_large/100; rpmSetPoint [2]=(Velocity/(curvefactor*1.1))*rpm_large/100; rpmSetPoint [3]=Velocity*rpm_large/100;
}
void stopAllMotors() { setMotorSpeed(frontLeftMotor,0); setMotorSpeed(frontRightMotor,0); setMotorSpeed(backLeftMotor,0); setMotorSpeed(backRightMotor,0); }
void setRPMTargetToZero() //reset stall detection { for (int i=0; i<4; i++) { rpmSetPoint [i]=0; } }
void readCompass_and_show() { sleep (200); readSensor(&compass); eraseDisplay(); displayBigTextLine(2, "Target: %4d", compass.offset); displayBigTextLine(6, "Abs: %4d", compass.heading); displayBigTextLine(10, "Rel: %4d", compass.relativeHeading); }
void checkMotorStall() { if ( rpmCounter>15) { writeDebugStreamLine("motor %d is stalled rpmMotorNow %f target %f", motorNumber, rpmMotorNow [motorNumber],rpmComparisionValue); stopAllMotors(); driveBack(30,50); rpmCounter=0; } }
//================= Positioning ====================================================================
//With the function "Positioning" the robot should set to the target angle as quick as possible. //After setting to the right heading, the robot has to adjust itself continuously during its journey using the function "driveAhead".
void Positioning() { setRPMTargetToZero(); //reset stall detection, set rpmSetPoint to zero
compass.offset=targetangle;
readCompass_and_show();
while (compass.relativeHeading < -2 || compass.relativeHeading >2) { readCompass_and_show(); if (compass.relativeHeading>0) { turnLeft(20,1.5,25); } if (compass.relativeHeading<0) { turnRight(20,1.5,25); } checkMotorStall(); } playSoundFile("Okay"); stopAllMotors(); sleep(250); }
//====================================================================================================
//===================== COMPASS CALIBRATION (Code by Xander Soldaat) ================================== // Start the calibration and complain loudly if something goes wrong void startCalibration() { if (!sensorCalibrate(&compass)) { eraseDisplay(); displayTextLine(1, "ERROR: Couldn't"); displayTextLine(2, "calibrate sensor."); displayTextLine(4, "Check connection"); displayTextLine(5, "and try again."); playSound(soundException); while(bSoundActive) sleep(1); sleep(5000); stopAllTasks(); } }
// Stop the calibration and complain loudly if somethign goes wrong void stopCalibration() { if (!sensorStopCalibrate(&compass)) { eraseDisplay(); displayTextLine(1, "ERROR: Calibration"); displayTextLine(2, "has failed."); displayTextLine(4, "Check connection"); displayTextLine(5, "and try again."); playSound(soundException); while(bSoundActive) sleep(1); sleep(5000); stopAllTasks(); } else { displayTextLine(1, "SUCCESS: "); displayTextLine(2, "Calibr. done."); playSound(soundUpwardTones); while(bSoundActive) sleep(1); sleep(5000); } }
///============================ HELP-TASKS =================================
task checkBattery() { while (true) { //eraseDisplay(); //displayBigTextLine(2, "Voltage: %2f", getBatteryVoltage()); if (getBatteryVoltage()<7.00) { clearSounds(); playSoundFile("Uh-oh"); } sleep(5000); } }
task stallMonitoringTask() {
setRPMTargetToZero(); //reset stall detection, set rpmSetPoint to zero
while (true) { rpmMotorNow [0]=getMotorRPM(frontLeftMotor); rpmMotorNow [1]=getMotorRPM(frontRightMotor); rpmMotorNow [2]=getMotorRPM(backLeftMotor); rpmMotorNow [3]=getMotorRPM(backRightMotor);
// If motors RPM fall below 10 percent of target RPM the motors stalled. Then call function "avoid obstacle" for (int i = 0; i < 4; i++) { rpmComparisionValue=rpm_threshold*rpm_large*rpmSetPoint [i]/10000;
if (rpmMotorNow [i]< rpmComparisionValue) { rpmCounter++; motorNumber=i; } } sleep(100); } }
task EnterButtonPressed() { while(true) { //While the left button (5) is pressed if (getButtonPress(2) == 1) { stopAllTasks(); } } }
//===========================================================================
task main()
{ // Initialise and configure struct and port initSensor(&compass, S1);
startTask (EnterButtonPressed);
startCalibration(); clearTimer (timer1); while (time1[T1] <12000) { turnLeft(20, 2.0, 25); //with a speed of 20% and a curvefactor of 2.0 and a motor power ratio of 25 } stopAllMotors();
stopCalibration();
resetSensorConn(S1); //important!! Otherwise the EV3 will freeze!
startTask (checkBattery);
startTask(stallMonitoringTask);
//================ DRIVING program The coordinates are not these ones I used in the video!!! ===================================== //Coordinates for the starting point lon1= 9.574336; lat1= 54.484920;
//1. waypoint lon2 = 9.574256; lat2 = 54.485194; calculate_distance(); calculate_targetangle(); Positioning(); wait1Msec(250); driveAhead(); playSoundFile("One");
//2. waypoint lon1=lon2; lat1=lat2; lon2 = 9.574743; lat2 = 54.485235; calculate_distance(); calculate_targetangle(); Positioning(); wait1Msec(250); driveAhead(); playSoundFile("Two");
//3. waypoint lon1=lon2; lat1=lat2; lon2 = 9.574801; lat2 = 54.484959; calculate_distance(); calculate_targetangle(); Positioning(); wait1Msec(250); driveAhead(); playSoundFile("Three");
//4. waypoint = starting point lon1=lon2; lat1=lat2; lon2 = 9.574336; lat2 = 54.484920; calculate_distance(); calculate_targetangle(); Positioning(); wait1Msec(250); driveAhead(); playSoundFile("Four");
wait1Msec (750); playSoundFile("Ready");
wait1Msec(1000);
resetSensorConn(S1); //important!! Otherwise the EV3 will freeze! stopAllTasks(); } |  |