Solution 1: Time based turning
This solution relies on a predetermined time value for how long to keep the drive servos moving in order to turn 90 degrees. Using the Parallax libraries, the code for this solution is short and straight forward. Success!
Wait, why didn't this work as well on carpet as hardwood? Why did Boe-bot do a 70 degree turn when the battery was running low?
This approach has a couple drawbacks. Since you have to determine how long the servo should spin at design time a number of factors will impact the run time need such as the specific servo power, remaining battery power and surface friction. Many of these factors aren't known as design and will change over time. Clearly this method introduces inaccuracy into our navigation!
Usage note on servo.h: Don't confuse servo_speed with servo_set. Using servo_speed to start and stop the servos will quickly chew through the available cogs leaving you pondering what went wrong unless you are inspecting the function return values for errors.
#include "simpletools.h"
#include "servo.h"
static int LEFT_MOTOR = 14;
static int RIGHT_MOTOR = 15;
void stop()
{
servo_set(LEFT_MOTOR, 1500);
servo_set(RIGHT_MOTOR, 1500);
}
void pre_determined_rotation_left_turn()
{
servo_set(LEFT_MOTOR, 1400);
servo_set(RIGHT_MOTOR, 1400);
pause(520);
stop();
}
int main()
{
pre_determined_rotation_left_turn();
}
Solution 2: Let's get some feedback
In the last post I introduced the compass module. Let's use the compass module to know the direction we started at and get feedback as we turn. In this solution, we turn a small increment and then check our position and repeat until we are at our goal. It's the robotic version of asking "are we there yet".
The real world isn't as precise as a float, so we can't directly compare our current heading to the desired heading to know if we are there yet. In fact, it can be a real challenge to work with floats in general. To step side this challenge, we introduce a delta value and accept success if our heading is within +/- the delta value from the desired heading.
This approach provides greater assurances of accuracy within a range. A trade off of this approach is that the many small steps produce a stuttering effect while turning. This translates into slower turning speed than a continuous approach.
#include "simpletools.h" // Include simple tools
#include "servo.h"
#include "compass3d.h" // Include compass3d header
#include "simplei2c.h"
//PIN ASSIGNMENTS//
static int COMPASS_SCL = 10;
static int COMPASS_SDA = 11;
static int LEFT_MOTOR = 14;
static int RIGHT_MOTOR = 15;
float getCompassHeading(i2c *bus)
{
int x, y, z; // Declare x, y, & z axis variables
compass_read(bus, &x, &y, &z); // Compass vals -> variables
int *px, *py, *pz;
px = &x;
py = &y;
pz = &z;
*px = x;
*py = y;
*pz = z;
float heading = atan2(x, y);
if(heading < 0)
{
heading += 2.0 * 3.14;
}
float headingDegrees = heading * 180/3.14;
return headingDegrees;
}
void stop_servo()
{
servo_set(LEFT_MOTOR, 1500);
servo_set(RIGHT_MOTOR, 1500);
}
void go_left(int duration)
{
servo_set(LEFT_MOTOR, 1400);
servo_set(RIGHT_MOTOR, 1400);
pause(duration);
stop_servo();
}
void turn_left_using_compass()
{
i2c *bus = i2c_newbus(COMPASS_SDA, COMPASS_SCL, 0); // New I2C bus SCL=P10, SDA=P11
compass_init(bus);
float newHeading = getCompassHeading(bus) + 90;
if(newHeading > 360)
{
newHeading = newHeading - 360;
}
printf("previous heading: %f\n",getCompassHeading(bus));
float tempHeading = getCompassHeading(bus);
static float delta = 3;
int keepTurning = 0;
printf("turning left to heading: %f\n",newHeading);
printf("margin of error: %f to ",(newHeading - delta));
printf("%f\n", (newHeading + delta));
while( keepTurning == 0)
{
go_left(40);
tempHeading = getCompassHeading(bus);
if( tempHeading > (newHeading - delta) && ( tempHeading < (newHeading + delta) ) )
{
printf("now at heading: %f\n",tempHeading);
//within margin of error, stop turning
keepTurning = 1;
}
}
print("left turn complete\n");
stop_servo();
}
int main()
{
turn_left_using_compass();
servo_stop();
}
Where to turn for future improvements
- A hybrid approach. Using the compass to take small steps provides greater accuracy at the cost of slower physical turning performance. Storing the sum of the steps the first time the function is run could be used to determine how big of a "long step" to make. Moving this from a design time to run time decision. Subsequent turns could then take that "long step" and then "small steps" as needed near the goal.
- Wheel encoders! Additional feedback from these sensors would enable precise turning of the wheels.
