Ian Colwell
/
RoboticsProject
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Project
by 
Thomas Elliott
Diff: main.cpp
- Revision:
- 11:521c3e8e6491
- Parent:
- 10:a3ecedc8d9d7
- Child:
- 12:3058f9fb09eb
--- a/main.cpp Tue Mar 12 19:40:04 2013 +0000
+++ b/main.cpp Fri Mar 15 21:33:02 2013 +0000
@@ -30,7 +30,9 @@
void GetSpeeds();
void SetLeftMotorSpeed(float u);
void SetRightMotorSpeed(float u);
-// void IRChecker();
+void DisplayMenu();
+void Ramp(float speed, unsigned int time, unsigned short motor);
+void IRChecker();
void BTInit();
// Global variables
@@ -48,6 +50,7 @@
float RecX[100]; // Record Integrator Output
float RecE[100]; // Record Error
int RecCount = 0; // Record Counter
+unsigned short action;
Mutex Var_Lock;
// global variables to eventually be removed
@@ -106,6 +109,124 @@
// ******** Main Thread ********
int main()
{
+ char c;
+
+ InitializeSystem();
+ BTInit();
+ BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
+ DisplayMenu();
+
+ //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
+ while(1)
+ {
+ if(BtS.readable())
+ {
+ c = BtS.getc();
+
+ // quit
+ if (c == 'q')
+ {
+ action = 0;
+ Ramp(0, 2000, 0);
+ DisplayMenu();
+ continue;
+ }
+
+ if (action == 0)
+ {
+ // choose a menu selection
+ switch(c)
+ {
+ case 'd':
+ action = 1;
+ break;
+ case 'w':
+ action = 2;
+ break;
+ case '0':
+ action = 3;
+ break;
+ case 'q':
+ action = 0;
+ break;
+ default:
+ BtS.printf("\n\r CCommand not recognized \n\r");
+ action = 0;
+ break;
+ }
+ continue;
+ }
+
+
+ if (action == 1)
+ {
+ // keyboard input to drive robot using wasd
+ switch(c)
+ {
+ case('w'):
+ userSetL = userSetL + 0.05;
+ userSetR = userSetR + 0.05;
+ break;
+ case('s'):
+ userSetL = userSetL - 0.05;
+ userSetR = userSetR - 0.05;
+ break;
+ case('a'):
+ userSetL = userSetL - 0.025;
+ userSetR = userSetR + 0.025;
+ break;
+ case('d'):
+ userSetL = userSetL + 0.025;
+ userSetR = userSetR - 0.025;
+ break;
+ }
+ }
+
+ if (action == 2)
+ {
+ // keyboard input to wall following
+ }
+ if (action == 3)
+ {
+ // keyboard input to debug mode
+ float newSpeed;
+ BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
+ BtS.scanf("%f", &newSpeed);
+
+ BtS.printf("%f", newSpeed);
+ Ramp(newSpeed, 1000, 0);
+ //userSetR = userSetL;
+ }
+
+
+
+ }// close if(BtS.readable())
+
+
+
+ if (action == 2)
+ {
+ IRChecker();
+ action = 0;
+ }
+ if (action == 3)
+ {
+ // display debug messages
+
+ //Var_Lock.lock();
+ BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
+ BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
+ BtS.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
+ BtS.printf("e L: %f R: %f \r\n", eL, eR);
+ BtS.printf("Ae L: %f R: %f \n\r", aeL, aeR);
+ BtS.printf("cSP L: %f R: %f \r\n\n", cSetL, cSetR);
+ //Var_Lock.unlock();
+
+ Thread::wait(2000);
+ }
+
+
+/*
InitializeSystem();
BTInit();
BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
@@ -151,23 +272,23 @@
char c;
//Var_Lock.lock();
- pc.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
- pc.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
- pc.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
- pc.printf("e L: %f R: %f \r\n", eL, eR);
- pc.printf("Ae L: %f R: %f \n\r", aeL, aeR);
- pc.printf("cSP L: %f R: %f \r\n\n", cSetL, cSetR);
+ BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
+ BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
+ BtS.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
+ BtS.printf("e L: %f R: %f \r\n", eL, eR);
+ BtS.printf("Ae L: %f R: %f \n\r", aeL, aeR);
+ BtS.printf("cSP L: %f R: %f \r\n\n", cSetL, cSetR);
//Var_Lock.unlock();
- if (pc.readable()){
- x=pc.getc();
- pc.putc(x); //Echo keyboard entry
+ if (BtS.readable()){
+ x=BtS.getc();
+ BtS.putc(x); //Echo keyboard entry
osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
}
- if(pc.readable())
+ if(BtS.readable())
{
- c = pc.getc();
+ c = BtS.getc();
if (c == 'r')
{
userSetL = 0.2;
@@ -176,11 +297,11 @@
{
if (RecCount == 100)
{
- pc.printf("\n\n\rRecV RecU RecX RecE \n\r");
+ BtS.printf("\n\n\rRecV RecU RecX RecE \n\r");
int i = 0;
for (i = 0; i < 100; i++)
{
- pc.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
+ BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
}
}
@@ -188,10 +309,10 @@
if (c == 'n')
{
- pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
- pc.scanf("%f", &userSetL);
+ BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
+ BtS.scanf("%f", &userSetL);
- pc.printf("%f", userSetL);
+ BtS.printf("%f", userSetL);
userSetR = userSetL;
}
}
@@ -296,7 +417,7 @@
Update PWM on-time register with abs(u);
Update the DIR pin on the LMD18200 with the sign of u.
*/
- if (userSetL == 0.2f)
+ /*if (userSetL == 0.8f)
{
if (RecCount < 100)
{
@@ -310,8 +431,8 @@
{
userSetL = 0;
}
-
- }
+
+ }*/
SetLeftMotorSpeed(cSetL);
SetRightMotorSpeed(cSetR);
}
@@ -331,7 +452,7 @@
void Watchdog(void const *n)
{
led3=1;
- pc.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
+ BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
}
// ******** Period Timer Interrupt Handler ********
@@ -518,21 +639,32 @@
fbSpeedR = -1;
}
}
-/*
-void IRChecker();
+
+void IRChecker()
{
float IRF, IRR;
+ char z;
// Read Sensors
- IRF = IRFront.read();
- IRR = IRRear.read();
- // Voltage Corresponding to nominal distance
+ while (z != 'q')
+ {
+ IRF = IRFront.read();
+ IRR = IRRear.read();
+ // Voltage Corresponding to nominal distance
+ BtS.printf("\n\rFront Sensor: %f", IRF);
+ BtS.printf("\n\rRear Sensor: %f\n\r", IRR);
+ if (BtS.readable())
+ {
+ z = BtS.getc();
+ }
+ Thread::wait(1000);
+ }
float Nominal = 3.0;
// Variable for turning robots
float Turn = 0.1;
// Ensure Robot isn't closer than 10cm from wall (reads no voltage)
- // Compare to nominal voltage and adjust
+ /* Compare to nominal voltage and adjust
if (IRF > Nominal && IRR > Nominal)
{
SetR = SetR - Turn;
@@ -563,11 +695,60 @@
SetRightMotorSpeed(SetR);
SetLeftMotorSpeed(SetL);
}
-
-} */
+ */
+}
void BTInit()
{
BtS.printf("AT+BTCANCEL\r\n");
BtS.printf("AT+BTSCAN\r\n");
+}
+
+void DisplayMenu()
+{
+ BtS.printf("\r\n --- Robot Initialization Complete --- \r\n\n");
+ BtS.printf(" Press the corresponding key to do something:\r\n");
+ BtS.printf("| Key | Action\n\r");
+ BtS.printf("|-----|-------\n\r");
+ BtS.printf("| d | Drive the robot using wasd keys\n\r");
+ BtS.printf("| w | Robot performs wall following\n\r");
+ BtS.printf("| 0 | Debug mode\n\r");
+ BtS.printf("| q | Quit current action, stop the robot, and return to this menu\n\r\n");
+
+}
+
+void Ramp(float speed, unsigned int time, unsigned short motor)
+{
+ const unsigned short steps = 20;
+ float changeL = (speed - userSetL)/steps;
+ float changeR = (speed - userSetR)/steps;
+ unsigned short i;
+ // calculate wait time (we wont worry too much about rounding errors)
+ unsigned int waitTime = time/steps;
+
+ for (i = 0; i < steps; i++)
+ {
+ //Thread::wait(200);
+ Thread::wait(waitTime);
+
+ if (motor == 0)
+ {
+ // change both speeds
+ userSetL += changeL;
+ userSetR += changeR;
+ continue;
+ }
+ if (motor == 1)
+ {
+ userSetL += changeL;
+ continue;
+ }
+ if (motor == 2)
+ {
+ userSetR += changeR;
+ }
+
+
+ }
+
}
\ No newline at end of file