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:
- 12:3058f9fb09eb
- Parent:
- 11:521c3e8e6491
- Child:
- 13:aaac0105a486
--- a/main.cpp Fri Mar 15 21:33:02 2013 +0000
+++ b/main.cpp Sun Mar 17 18:44:47 2013 +0000
@@ -73,7 +73,7 @@
SCK Quad Enc 7|-|
SPI Start Quad E 8|-|
SPI Reset Quad E 9|-|
-
+Emergency Stop 10|-|
Bluetooth tx 13|-|28
Bluetooth rx 14|-|27
15|-|26 Brake, Left Motor, M1
@@ -100,7 +100,7 @@
SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA
DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
-InterruptIn Bumper(p10); // External interrupt pin
+InterruptIn EmerStop(p10); // External interrupt pin
AnalogIn IRFront(p19); // Front IR Ranger Input
AnalogIn IRRear(p20); // Rear IR Ranger Input
Ticker PeriodicInt;
@@ -112,7 +112,6 @@
char c;
InitializeSystem();
- BTInit();
BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
DisplayMenu();
@@ -127,7 +126,7 @@
if (c == 'q')
{
action = 0;
- Ramp(0, 2000, 0);
+ Ramp(0, 1000, 0);
DisplayMenu();
continue;
}
@@ -139,6 +138,7 @@
{
case 'd':
action = 1;
+ BtS.printf("\n\rTap w-a-s-d keys for differential speed control\r\nPress 'q' to quit \r\n");
break;
case 'w':
action = 2;
@@ -150,14 +150,13 @@
action = 0;
break;
default:
- BtS.printf("\n\r CCommand not recognized \n\r");
+ BtS.printf("\n\r Command not recognized \n\r");
action = 0;
break;
}
continue;
}
-
if (action == 1)
{
// keyboard input to drive robot using wasd
@@ -180,6 +179,7 @@
userSetR = userSetR - 0.025;
break;
}
+ continue;
}
if (action == 2)
@@ -196,14 +196,10 @@
BtS.printf("%f", newSpeed);
Ramp(newSpeed, 1000, 0);
//userSetR = userSetL;
- }
-
-
-
+ }
}// close if(BtS.readable())
-
if (action == 2)
{
IRChecker();
@@ -213,14 +209,19 @@
{
// display debug messages
- //Var_Lock.lock();
+ float IRF, IRR;
+ IRF = IRFront.read();
+ IRR = IRRear.read();
+
+ 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();
+ BtS.printf("cSP L: %f R: %f \n\r", cSetL, cSetR);
+ BtS.printf("IR F: %f R: %f \n\r\n", IRF, IRR);
+ Var_Lock.unlock();
Thread::wait(2000);
}
@@ -237,55 +238,10 @@
{
Thread::wait(20);
- if (BtS.readable())
- {
- x = BtS.getc();
-
- switch(x)
- {
- 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;
- case('b'):
- // Eventually ramp down to 0, RampDown();
- userSetL = 0;
- userSetR = 0;
- break;
- }
-
- }
+
- /*
- char c;
-
- //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();
- if (BtS.readable()){
- x=BtS.getc();
- BtS.putc(x); //Echo keyboard entry
- osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
-
- }
+
if(BtS.readable())
{
c = BtS.getc();
@@ -332,8 +288,8 @@
float prevu1, prevu2;
//float eL = 0;
//float eR = 0;
- const unsigned short maxError = 1000;
- const unsigned short maxAcc = 10000;
+ const unsigned short maxError = 1;
+ const unsigned short maxAcc = 10;
// Kp = 0.1, Ki = 0.5
const float Kp = 0.1f;
const float Ki = 0.5f;
@@ -347,11 +303,8 @@
// calculate error
eL = userSetL - fbSpeedL;
eR = userSetR - fbSpeedR;
- //eL = -eL;
- //eR = -eR;
- // prevent overflow / bound the error
- /*
+ // prevent overflow / bound the error
if (eL > maxError)
{
eL = maxError;
@@ -367,8 +320,7 @@
if (eR < -maxError);
{
eR = -maxError;
- }
- */
+ }
// accumulated error (integration)
if (prevu1 < 1 && prevu1 > -1)
@@ -381,7 +333,6 @@
}
// bound the accumulatd error
- /*
if (aeL > maxAcc)
{
aeL = maxAcc;
@@ -398,7 +349,6 @@
{
aeR = -maxAcc;
}
- */
u1 = Kp*eL + Ki*aeL;
u2 = Kp*eR + Ki*aeR;
@@ -406,17 +356,7 @@
cSetL = userSetL + u1;
cSetR = userSetR + u2;
- //u1 = -u1;
- //u2 = -u2;
- // Is signaled by a periodic timer interrupt handler
- /*
- Read incremental position, dPosition, and time interval from the QEI.
- e = Setpoint – dPosition // e is the velocity error
- xState = xState + e; // x is the Euler approximation to the integral of e.
- u = Kp*e + Ki*xState; // u is the control signal
- Update PWM on-time register with abs(u);
- Update the DIR pin on the LMD18200 with the sign of u.
- */
+ // data recording code
/*if (userSetL == 0.8f)
{
if (RecCount < 100)
@@ -433,6 +373,7 @@
}
}*/
+
SetLeftMotorSpeed(cSetL);
SetRightMotorSpeed(cSetR);
}
@@ -467,13 +408,14 @@
osSignalSet(ExtCollision,0x1);
}
+
// --- Initialization Functions
void InitializeSystem()
{
led3=0;
led4=0;
- Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
+ EmerStop.fall(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
// Start execution of the Threads
PiControl = osThreadCreate(osThread(PiControlThread), NULL);
@@ -483,6 +425,7 @@
InitializePWM();
InitializeEncoder();
+ BTInit();
}
void InitializePWM()
@@ -504,6 +447,11 @@
// starting at base address 0 within the peripheral.
}
+void BTInit()
+{
+ BtS.printf("AT+BTCANCEL\r\n");
+ BtS.printf("AT+BTSCAN\r\n");
+}
// --- Other Functions
void SetLeftMotorSpeed(float u)
@@ -584,37 +532,13 @@
wait_us(5);
SpiStart = 0;
DE0.write(0x8004);
+
// read in 4 16-bit words
Var_Lock.lock();
dPositionLeft = DE0.write(Dummy); // Read QEI-0 position register
dTimeLeft = DE0.write(Dummy); // Read QE-0 time interval register
dPositionRight = DE0.write(Dummy); // Read QEI-1 position register
dTimeRight = DE0.write(Dummy); // Read QEI-1 time interval register
-
- // figure out which direction the motor is turning
- /*
- if (dPositionLeft > 32767)
- {
- // turning backwards
- *leftSpeed = -(65535 - dPositionLeft)/leftMaxPos;
- }
- else
- {
- // turning forwards
- *leftSpeed = dPositionLeft/leftMaxPos;
- }
-
- if (dPositionRight > 32767)
- {
- // turning backwards
- *rightSpeed = -(65535 - dPositionRight)/rightMaxPos;
- }
- else
- {
- // turning forwards
- *rightSpeed = dPositionRight/rightMaxPos;
- }
- */
Var_Lock.unlock();
// calcspeed
@@ -698,23 +622,15 @@
*/
}
-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("\r\n\nPress the corresponding key to do something:\r\n");
BtS.printf("| Key | Action\n\r");
- BtS.printf("|-----|-------\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");
-
+ 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)
@@ -746,9 +662,6 @@
if (motor == 2)
{
userSetR += changeR;
- }
-
-
- }
-
+ }
+ }
}
\ No newline at end of file