Yo Shua
Turning Robot with IMU and calibrate the straight movement
Dependencies: 4DGL-uLCD-SE LSM9DS0 Motor PinDetect mbed
Revision 1:5bf45482441d, committed 2015-10-20
- Comitter:
- yoshua0207
- Date:
- Tue Oct 20 04:23:32 2015 +0000
- Parent:
- 0:31128b41490c
- Commit message:
- Version 1.2
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 31128b41490c -r 5bf45482441d main.cpp
--- a/main.cpp Mon Oct 19 04:20:24 2015 +0000
+++ b/main.cpp Tue Oct 20 04:23:32 2015 +0000
@@ -51,6 +51,7 @@
int initialHeading = 0;
int state = 0;
int difference = 0;
+int counter = 0;
void setup()
{
uLCD.cls();
@@ -114,15 +115,16 @@
{
imu.readMag();
myLED3 = 1;
- uLCD.locate(1,5);
- uLCD.printf("Turning Right \n");
- mA.speed(0.2);
- mB.speed(-0.2);
+// uLCD.locate(1,5);
+// uLCD.printf("Turning Right \n");
+ mA.speed(0.25);
+ mB.speed(-0.25);
heading = static_cast<int> (imu.calcHeading());
- uLCD.printf("H : %d \n",heading);
+// uLCD.printf("H : %d \n",heading);
degree = heading - initialHeading;
degree = turningDegree - degree;
- uLCD.printf("Degree = %d \n",degree);
+ uLCD.locate(1,5);
+ uLCD.printf("Degree Left = %d \n",degree);
uLCD.printf("Heading = %d \n",heading);
}
@@ -130,15 +132,16 @@
{
imu.readMag();
myLED3 = 1;
- uLCD.locate(1,5);
- uLCD.printf("Turning Left \n");
- mA.speed(-0.2);
- mB.speed(0.2);
+// uLCD.locate(1,5);
+// uLCD.printf("Turning Left \n");
+ mA.speed(-0.25);
+ mB.speed(0.25);
heading = static_cast<int> (imu.calcHeading());
- uLCD.printf("H : %d \n",heading);
+// uLCD.printf("H : %d \n",heading);
degree = heading - initialHeading;
degree = turningDegree - degree;
- uLCD.printf("Degree = %d \n",degree);
+ uLCD.locate(1,5);
+ uLCD.printf("Degree Left = %d \n",degree);
uLCD.printf("Heading = %d \n",heading);
}
@@ -172,14 +175,14 @@
//Setting mode
while(settingMode == 0)
{
- uLCD.locate(1,2);
+ uLCD.locate(1,1);
if (mode1 == SETX)
{
- uLCD.printf("Setting X coordinate");
+ uLCD.printf("Setting X Coord");
}
else if (mode1 == SETY)
{
- uLCD.printf("Setting Y Coordinate");
+ uLCD.printf("Setting Y Coord");
}
else if (mode1 == GO)
{
@@ -200,9 +203,9 @@
initialHeading = static_cast<int>(imu.calcHeading());
angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI;
finalDegree = initialHeading + angle;
- uLCD.locate(4,9);
+ uLCD.locate(1,9);
uLCD.printf("Distance: %5.2F Feet \n\r", distance);
- uLCD.locate(4,11);
+ uLCD.locate(1,11);
uLCD.printf("Turning: %5.2F Degree \n\r", angle);
settingMode = 1;
@@ -214,7 +217,7 @@
while(settingMode == 1)
{
uLCD.cls();
- uLCD.printf("Setting mode 1");
+// uLCD.printf("Setting mode 1");
imu.readMag();
initialHeading = static_cast<int> (imu.calcHeading());
uLCD.locate(1,3);
@@ -229,9 +232,10 @@
while (degree > -2 && degree < 180)
{
turnRight();
- wait(0.2);
+
mA.speed(0);
mB.speed(0);
+ wait(0.4);
}
settingMode = 2;
@@ -241,9 +245,10 @@
while (degree < 2 && degree > -180 )
{
turnLeft();
- wait(0.2);
+
mA.speed(0);
- mB.speed(0);
+ mB.speed(0);
+ wait(0.4);
}
settingMode = 2;
}
@@ -253,28 +258,37 @@
imu.readMag();
initialHeading = static_cast<int> (imu.calcHeading());
timer.start();
+ uLCD.cls();
while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2)
{
- uLCD.locate(1,7);
- uLCD.printf("Time : %d \n",timer.read_ms());
+
+
+// uLCD.printf("Time : %d \n",timer.read_ms());
mA.speed(0.4);
mB.speed(0.4);
imu.readMag();
heading = static_cast<int> (imu.calcHeading());
- difference = heading - initialHeading;
- uLCD.locate(1,3);
- uLCD.printf("Heading : %d \n",heading);
- if( difference <-2)
+ difference = heading - finalDegree;
+ uLCD.locate(5,6);
+ uLCD.printf("Go!");
+ while( difference <-2 &&timer.read_ms() < static_cast<int>(seconds) )
{
+
mA.speed(0.5);
mB.speed(0.4);
imu.readMag();
heading = static_cast<int> (imu.calcHeading());
difference = heading - finalDegree;
uLCD.locate(1,3);
- uLCD.printf("Heading : %d \n",heading);
+ uLCD.printf("Difference : %d \n",difference);
+ uLCD.locate(1,4);
+ uLCD.printf("Turning Right \n");
+ uLCD.locate(1,5);
+ uLCD.printf("Counter = %d \n",counter);
+ counter++;
+// wait(0.2);
}
- if(difference >2)
+ while(difference >2 && timer.read_ms() < static_cast<int>(seconds))
{
mA.speed(0.4);
mB.speed(0.5);
@@ -282,7 +296,13 @@
heading = static_cast<int> (imu.calcHeading());
difference = heading - finalDegree;
uLCD.locate(1,3);
- uLCD.printf("Heading : %d \n",heading);
+ uLCD.printf("Difference : %d \n",difference);
+ uLCD.locate(1,4);
+ uLCD.printf("Turning Left \n");
+ uLCD.locate(1,5);
+ uLCD.printf("Counter = %d \n",counter);
+ counter++;
+// wait(0.2);
}
}
mA.speed(0);