View unanswered posts | View active topics It is currently Sat Nov 17, 2018 7:52 am






Reply to topic  [ 6 posts ] 
NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot 
Author Message
Rookie
User avatar

Joined: Mon Apr 08, 2013 12:50 pm
Posts: 30
Post NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot
Let me introduce my new project "Grasshopper 1.0". I was inspired by Steve Hassenplugs "Green Monsters" which are made for outdoor using (http://www.teamhassenplug.org/robots/GreenMonster/).
Steve Hassenplug had to use the NXT-brick for his robot, because EV3 wasn't allready developed. But watch the great videos of his "Green Monsters" on YouTube
and you will see, how well they can drive over grass and streets!
The advantage of EV3 compared to the NXT is that it can handle more sensor data in time than the NXT-brick and the EV3 is able to control
four motors instead of three as the NXT can. The four wheel drive allows even better outdoor performance.
This "Grasshopper 1.0" consists of four NXT-motors, one NXT-ultrasonic sensor and a hitechnic compass sensor, controlled by an EV3-brick.
On its back you can see a mounting platform for a Dexter GPS. I don't use it yet, because the GPS accurancy lies between 5 and 10 meters!
Because my plot of land isn't big as many in the USA, it is senseless to use it at the moment. But I am going to expand the program for using the
Dexter GPS in future and will try it on a larger grass plane.

So what this version of RobotC (http://www.robotc.net/) program does is the following:
At first the HT-compass has to be calibrated. For that the robot drives in a circle about 360 degrees. After that it calculates the heading and
distance of a position point to another that you can pick from Google Earth. So you can define waypoints which the robot will head for.
You can set these points as lon (longitude) and lat (latitude) coordinates in the main task () as you can see in my example program.
In this version the robot uses only its compass sensor and the odometry function of motors to navigate. During robots autonomous journey it
compares its currently heading with the calculated target heading and corrects its heading if necessary by driving a curve to the left or right.
The covered distance is measured by the motorencoder of back left motor.
In addition it uses the ultrasonic sensor to detect obstacles in front of it.
If the robot detects an obstacle it will drive back and turn a curve, trying to avoid it.
In a special task the program continously checks stall of motors. So, if one or more wheels stall, it will drive back a small distance to escape the trap.
After avoiding the obstacle or escaping the trap the robot will continue its journey to the fixed waypoint.
A further task checks the battery current. If it falls below 7V the robot says "Uh-oh!".
Before starting journey and after reaching a waypoint the robot will drive in position for the right target heading (function positioning()). To avoid drifting or slipping
at the beginning of the journey, the speed is reduced to half of max. speed for 2 seconds before the robot drives with full speed. In addition the front
motors are synchronized.
Using the "enter"-button of EV3 you can stop the program. Don't use the "back"-button, because that will freeze programm. I don't know why?!

Special thanks to Xander Soldaat who supported me with drivers and valuable hints!!

You can rebuild this robot by using the LEGO Designer (http://ldd.lego.com/de-de/) and my lxf-file (https://www.amazon.de/clouddrive/share/ ... Zz8u4JIy3Y)

In addition use the enclosed fotos and the pictures. If you have some improvements for me, don't hesitate to share!


Best regards,

Sigtrygg



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();
}


Attachments:
File comment: picture 2
Grasshopper_2.JPG
Grasshopper_2.JPG [ 329.94 KiB | Viewed 12060 times ]
File comment: picture 1
Grasshopper_1.JPG
Grasshopper_1.JPG [ 345.61 KiB | Viewed 12060 times ]
File comment: LEGO-picture
Grasshopper Vers. 1.0_EV3_water.jpg
Grasshopper Vers. 1.0_EV3_water.jpg [ 100.43 KiB | Viewed 12060 times ]
Sat Apr 11, 2015 11:11 am
Profile
Site Admin
Site Admin
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3654
Location: Rotterdam, The Netherlands
Post Re: NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot
Wow, that thing is fast! Thank you for sharing the video, photos and code!

Makes me wish I had those large wheels :)

= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Mon Apr 13, 2015 1:29 am
Profile WWW
Rookie
User avatar

Joined: Mon Apr 08, 2013 12:50 pm
Posts: 30
Post Re: NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot
Hello Xander!

I got some 160 mm wheels from ebay. They were sold with the LEGO set:

LEGO Mobile Devastator Set 8108
Theme: LEGO Exo-Force
Set Number: 8108-1
Item No: 4498736

The specification of the wheel is:
LEGO Lime Wheel Ø160 (59521)

Best regards,

Sigtrygg


Sun Apr 19, 2015 5:36 pm
Profile
Site Admin
Site Admin
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3654
Location: Rotterdam, The Netherlands
Post Re: NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot
Thanks for the part number info :)

= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue Apr 21, 2015 12:28 am
Profile WWW
Rookie
User avatar

Joined: Thu Apr 23, 2015 1:29 am
Posts: 1
Post Re: NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot
Wow that's awesome dude! :bigthumb: :bigthumb: :bigthumb:

_________________
Genius will live and thrive without training, but it does not the less reward the watering-pot and butterfly knife


Last edited by darwingregory on Thu Apr 23, 2015 10:36 pm, edited 1 time in total.



Thu Apr 23, 2015 1:49 am
Profile YIM
Rookie
User avatar

Joined: Mon Apr 08, 2013 12:50 pm
Posts: 30
Post Re: NEW PROJECT: Grasshopper Vers. 1.0 EV3 outdoor robot
Thank you, darwingregory!

I am glad that you like it! :)


Thu Apr 23, 2015 1:59 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 6 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.