Ian Colwell / Mbed 2 deprecated 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.

Dependencies:   mbed-rtos mbed

Fork of Project by Thomas Elliott

main.cpp@11:521c3e8e6491, 2013-03-15 (annotated)

Committer:
LtBarbershop
Date:
Fri Mar 15 21:33:02 2013 +0000
Revision:
11:521c3e8e6491
Parent:
10:a3ecedc8d9d7
Child:
12:3058f9fb09eb
March 15, Completely bluetooth controlled, menu (debug, wasd, IR value), ramp function.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IanTheMBEDMaster 2:1c5cc180812d 1 // Robot Control Code
IanTheMBEDMaster 2:1c5cc180812d 2 // Tom Elliott and Ian Colwell
IanTheMBEDMaster 2:1c5cc180812d 3
IanTheMBEDMaster 2:1c5cc180812d 4
LtBarbershop 1:3a40c918ff41 5 #include "mbed.h"
LtBarbershop 1:3a40c918ff41 6 #include "rtos.h"
LtBarbershop 1:3a40c918ff41 7
IanTheMBEDMaster 2:1c5cc180812d 8 // --- Constants
IanTheMBEDMaster 2:1c5cc180812d 9 #define Dummy 0
IanTheMBEDMaster 6:5200ce9fa5a7 10
IanTheMBEDMaster 7:751d5e3e9cab 11 //#define PWMPeriod 0.0005 // orignally 0.001 (gave a high pitched whine)
IanTheMBEDMaster 7:751d5e3e9cab 12 const float PWMPeriod = 0.00005;
LtBarbershop 5:7108ac9e8182 13 // Control Update = 50ms (time should be 609/610) (if we want to change this we also have to change the way feedback speed is calculated)
IanTheMBEDMaster 6:5200ce9fa5a7 14 //#define ControlUpdate 0.05
IanTheMBEDMaster 6:5200ce9fa5a7 15 const float ControlUpdate = 0.05;
LtBarbershop 4:03bf5bdca9fb 16 #define EncoderTime 610
LtBarbershop 5:7108ac9e8182 17
LtBarbershop 1:3a40c918ff41 18
IanTheMBEDMaster 2:1c5cc180812d 19 // --- Function prototypes
LtBarbershop 1:3a40c918ff41 20 void PiControllerISR(void);
LtBarbershop 1:3a40c918ff41 21 void WdtFaultISR(void);
LtBarbershop 1:3a40c918ff41 22 void ExtCollisionISR(void);
LtBarbershop 1:3a40c918ff41 23 void PiControlThread(void const *argument);
LtBarbershop 1:3a40c918ff41 24 void ExtCollisionThread(void const *argument);
LtBarbershop 1:3a40c918ff41 25 void Watchdog(void const *n);
IanTheMBEDMaster 2:1c5cc180812d 26 void InitializeSystem();
IanTheMBEDMaster 2:1c5cc180812d 27 void InitializeEncoder();
IanTheMBEDMaster 2:1c5cc180812d 28 void InitializePWM();
IanTheMBEDMaster 2:1c5cc180812d 29 void PwmSetOut(float d, float T);
IanTheMBEDMaster 7:751d5e3e9cab 30 void GetSpeeds();
IanTheMBEDMaster 2:1c5cc180812d 31 void SetLeftMotorSpeed(float u);
IanTheMBEDMaster 2:1c5cc180812d 32 void SetRightMotorSpeed(float u);
LtBarbershop 11:521c3e8e6491 33 void DisplayMenu();
LtBarbershop 11:521c3e8e6491 34 void Ramp(float speed, unsigned int time, unsigned short motor);
LtBarbershop 11:521c3e8e6491 35 void IRChecker();
LtBarbershop 10:a3ecedc8d9d7 36 void BTInit();
LtBarbershop 1:3a40c918ff41 37
IanTheMBEDMaster 6:5200ce9fa5a7 38 // Global variables
LtBarbershop 5:7108ac9e8182 39 float u1 = 0;
LtBarbershop 5:7108ac9e8182 40 float u2 = 0;
IanTheMBEDMaster 7:751d5e3e9cab 41 float cSetL = 0;
IanTheMBEDMaster 7:751d5e3e9cab 42 float cSetR = 0;
LtBarbershop 5:7108ac9e8182 43 float userSetL = 0;
LtBarbershop 5:7108ac9e8182 44 float userSetR = 0;
LtBarbershop 4:03bf5bdca9fb 45 int startup = 0;
LtBarbershop 4:03bf5bdca9fb 46 float aeL = 0;
LtBarbershop 4:03bf5bdca9fb 47 float aeR = 0;
LtBarbershop 8:d65cba3bfc0e 48 float RecV[100]; // Record Feedback Speed
LtBarbershop 8:d65cba3bfc0e 49 float RecU[100]; // Record Motor Input
LtBarbershop 8:d65cba3bfc0e 50 float RecX[100]; // Record Integrator Output
LtBarbershop 8:d65cba3bfc0e 51 float RecE[100]; // Record Error
LtBarbershop 8:d65cba3bfc0e 52 int RecCount = 0; // Record Counter
LtBarbershop 11:521c3e8e6491 53 unsigned short action;
IanTheMBEDMaster 6:5200ce9fa5a7 54 Mutex Var_Lock;
IanTheMBEDMaster 6:5200ce9fa5a7 55
IanTheMBEDMaster 6:5200ce9fa5a7 56 // global variables to eventually be removed
IanTheMBEDMaster 7:751d5e3e9cab 57 short dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
IanTheMBEDMaster 6:5200ce9fa5a7 58 float fbSpeedL, fbSpeedR;
LtBarbershop 5:7108ac9e8182 59 float eL = 0;
LtBarbershop 5:7108ac9e8182 60 float eR = 0;
IanTheMBEDMaster 2:1c5cc180812d 61
IanTheMBEDMaster 2:1c5cc180812d 62 // --- Processes and threads
LtBarbershop 1:3a40c918ff41 63 int32_t SignalPi, SignalWdt, SignalExtCollision;
LtBarbershop 1:3a40c918ff41 64 osThreadId PiControl,WdtFault,ExtCollision;
LtBarbershop 1:3a40c918ff41 65 osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 66 osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 67 osTimerDef(Wdtimer, Watchdog);
LtBarbershop 1:3a40c918ff41 68
IanTheMBEDMaster 2:1c5cc180812d 69 /* PIN-OUT
IanTheMBEDMaster 2:1c5cc180812d 70
IanTheMBEDMaster 2:1c5cc180812d 71 MOSI Quad Enc 5|-|
IanTheMBEDMaster 2:1c5cc180812d 72 MISO Quad Enc 6|-|
IanTheMBEDMaster 2:1c5cc180812d 73 SCK Quad Enc 7|-|
IanTheMBEDMaster 2:1c5cc180812d 74 SPI Start Quad E 8|-|
IanTheMBEDMaster 2:1c5cc180812d 75 SPI Reset Quad E 9|-|
IanTheMBEDMaster 2:1c5cc180812d 76
IanTheMBEDMaster 2:1c5cc180812d 77 Bluetooth tx 13|-|28
IanTheMBEDMaster 2:1c5cc180812d 78 Bluetooth rx 14|-|27
LtBarbershop 4:03bf5bdca9fb 79 15|-|26 Brake, Left Motor, M1
LtBarbershop 4:03bf5bdca9fb 80 16|-|25 Dir, Left Motor, M1
LtBarbershop 4:03bf5bdca9fb 81 17|-|24 PWM, Left Motor, M1
LtBarbershop 4:03bf5bdca9fb 82 18|-|23 Brake, Right Motor, M2
LtBarbershop 9:0eb7b173d6c3 83 Front IR 19|-|22 Dir, Right Motor, M2
LtBarbershop 9:0eb7b173d6c3 84 Rear IR 20|-|21 PWM, Right Motor, M2
IanTheMBEDMaster 2:1c5cc180812d 85 */
IanTheMBEDMaster 2:1c5cc180812d 86
IanTheMBEDMaster 2:1c5cc180812d 87 // --- IO Port Configuration
LtBarbershop 1:3a40c918ff41 88 DigitalOut led1(LED1);
LtBarbershop 1:3a40c918ff41 89 DigitalOut led2(LED2);
LtBarbershop 1:3a40c918ff41 90 DigitalOut led3(LED3);
LtBarbershop 1:3a40c918ff41 91 DigitalOut led4(LED4);
LtBarbershop 8:d65cba3bfc0e 92 DigitalOut dirR(p22);
LtBarbershop 8:d65cba3bfc0e 93 DigitalOut brakeR(p23);
LtBarbershop 8:d65cba3bfc0e 94 PwmOut PwmR(p21);
LtBarbershop 8:d65cba3bfc0e 95 DigitalOut dirL(p25);
LtBarbershop 8:d65cba3bfc0e 96 DigitalOut brakeL(p26);
LtBarbershop 8:d65cba3bfc0e 97 PwmOut PwmL(p24);
LtBarbershop 10:a3ecedc8d9d7 98 Serial BtS(p13, p14); // (tx, rx) for PC serial channel
LtBarbershop 1:3a40c918ff41 99 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
IanTheMBEDMaster 2:1c5cc180812d 100 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA
IanTheMBEDMaster 2:1c5cc180812d 101 DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
IanTheMBEDMaster 2:1c5cc180812d 102 DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
IanTheMBEDMaster 2:1c5cc180812d 103 InterruptIn Bumper(p10); // External interrupt pin
LtBarbershop 9:0eb7b173d6c3 104 AnalogIn IRFront(p19); // Front IR Ranger Input
LtBarbershop 9:0eb7b173d6c3 105 AnalogIn IRRear(p20); // Rear IR Ranger Input
LtBarbershop 1:3a40c918ff41 106 Ticker PeriodicInt;
IanTheMBEDMaster 2:1c5cc180812d 107
LtBarbershop 1:3a40c918ff41 108
LtBarbershop 1:3a40c918ff41 109 // ******** Main Thread ********
IanTheMBEDMaster 2:1c5cc180812d 110 int main()
IanTheMBEDMaster 2:1c5cc180812d 111 {
LtBarbershop 11:521c3e8e6491 112 char c;
LtBarbershop 11:521c3e8e6491 113
LtBarbershop 11:521c3e8e6491 114 InitializeSystem();
LtBarbershop 11:521c3e8e6491 115 BTInit();
LtBarbershop 11:521c3e8e6491 116 BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
LtBarbershop 11:521c3e8e6491 117 DisplayMenu();
LtBarbershop 11:521c3e8e6491 118
LtBarbershop 11:521c3e8e6491 119 //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
LtBarbershop 11:521c3e8e6491 120 while(1)
LtBarbershop 11:521c3e8e6491 121 {
LtBarbershop 11:521c3e8e6491 122 if(BtS.readable())
LtBarbershop 11:521c3e8e6491 123 {
LtBarbershop 11:521c3e8e6491 124 c = BtS.getc();
LtBarbershop 11:521c3e8e6491 125
LtBarbershop 11:521c3e8e6491 126 // quit
LtBarbershop 11:521c3e8e6491 127 if (c == 'q')
LtBarbershop 11:521c3e8e6491 128 {
LtBarbershop 11:521c3e8e6491 129 action = 0;
LtBarbershop 11:521c3e8e6491 130 Ramp(0, 2000, 0);
LtBarbershop 11:521c3e8e6491 131 DisplayMenu();
LtBarbershop 11:521c3e8e6491 132 continue;
LtBarbershop 11:521c3e8e6491 133 }
LtBarbershop 11:521c3e8e6491 134
LtBarbershop 11:521c3e8e6491 135 if (action == 0)
LtBarbershop 11:521c3e8e6491 136 {
LtBarbershop 11:521c3e8e6491 137 // choose a menu selection
LtBarbershop 11:521c3e8e6491 138 switch(c)
LtBarbershop 11:521c3e8e6491 139 {
LtBarbershop 11:521c3e8e6491 140 case 'd':
LtBarbershop 11:521c3e8e6491 141 action = 1;
LtBarbershop 11:521c3e8e6491 142 break;
LtBarbershop 11:521c3e8e6491 143 case 'w':
LtBarbershop 11:521c3e8e6491 144 action = 2;
LtBarbershop 11:521c3e8e6491 145 break;
LtBarbershop 11:521c3e8e6491 146 case '0':
LtBarbershop 11:521c3e8e6491 147 action = 3;
LtBarbershop 11:521c3e8e6491 148 break;
LtBarbershop 11:521c3e8e6491 149 case 'q':
LtBarbershop 11:521c3e8e6491 150 action = 0;
LtBarbershop 11:521c3e8e6491 151 break;
LtBarbershop 11:521c3e8e6491 152 default:
LtBarbershop 11:521c3e8e6491 153 BtS.printf("\n\r CCommand not recognized \n\r");
LtBarbershop 11:521c3e8e6491 154 action = 0;
LtBarbershop 11:521c3e8e6491 155 break;
LtBarbershop 11:521c3e8e6491 156 }
LtBarbershop 11:521c3e8e6491 157 continue;
LtBarbershop 11:521c3e8e6491 158 }
LtBarbershop 11:521c3e8e6491 159
LtBarbershop 11:521c3e8e6491 160
LtBarbershop 11:521c3e8e6491 161 if (action == 1)
LtBarbershop 11:521c3e8e6491 162 {
LtBarbershop 11:521c3e8e6491 163 // keyboard input to drive robot using wasd
LtBarbershop 11:521c3e8e6491 164 switch(c)
LtBarbershop 11:521c3e8e6491 165 {
LtBarbershop 11:521c3e8e6491 166 case('w'):
LtBarbershop 11:521c3e8e6491 167 userSetL = userSetL + 0.05;
LtBarbershop 11:521c3e8e6491 168 userSetR = userSetR + 0.05;
LtBarbershop 11:521c3e8e6491 169 break;
LtBarbershop 11:521c3e8e6491 170 case('s'):
LtBarbershop 11:521c3e8e6491 171 userSetL = userSetL - 0.05;
LtBarbershop 11:521c3e8e6491 172 userSetR = userSetR - 0.05;
LtBarbershop 11:521c3e8e6491 173 break;
LtBarbershop 11:521c3e8e6491 174 case('a'):
LtBarbershop 11:521c3e8e6491 175 userSetL = userSetL - 0.025;
LtBarbershop 11:521c3e8e6491 176 userSetR = userSetR + 0.025;
LtBarbershop 11:521c3e8e6491 177 break;
LtBarbershop 11:521c3e8e6491 178 case('d'):
LtBarbershop 11:521c3e8e6491 179 userSetL = userSetL + 0.025;
LtBarbershop 11:521c3e8e6491 180 userSetR = userSetR - 0.025;
LtBarbershop 11:521c3e8e6491 181 break;
LtBarbershop 11:521c3e8e6491 182 }
LtBarbershop 11:521c3e8e6491 183 }
LtBarbershop 11:521c3e8e6491 184
LtBarbershop 11:521c3e8e6491 185 if (action == 2)
LtBarbershop 11:521c3e8e6491 186 {
LtBarbershop 11:521c3e8e6491 187 // keyboard input to wall following
LtBarbershop 11:521c3e8e6491 188 }
LtBarbershop 11:521c3e8e6491 189 if (action == 3)
LtBarbershop 11:521c3e8e6491 190 {
LtBarbershop 11:521c3e8e6491 191 // keyboard input to debug mode
LtBarbershop 11:521c3e8e6491 192 float newSpeed;
LtBarbershop 11:521c3e8e6491 193 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 11:521c3e8e6491 194 BtS.scanf("%f", &newSpeed);
LtBarbershop 11:521c3e8e6491 195
LtBarbershop 11:521c3e8e6491 196 BtS.printf("%f", newSpeed);
LtBarbershop 11:521c3e8e6491 197 Ramp(newSpeed, 1000, 0);
LtBarbershop 11:521c3e8e6491 198 //userSetR = userSetL;
LtBarbershop 11:521c3e8e6491 199 }
LtBarbershop 11:521c3e8e6491 200
LtBarbershop 11:521c3e8e6491 201
LtBarbershop 11:521c3e8e6491 202
LtBarbershop 11:521c3e8e6491 203 }// close if(BtS.readable())
LtBarbershop 11:521c3e8e6491 204
LtBarbershop 11:521c3e8e6491 205
LtBarbershop 11:521c3e8e6491 206
LtBarbershop 11:521c3e8e6491 207 if (action == 2)
LtBarbershop 11:521c3e8e6491 208 {
LtBarbershop 11:521c3e8e6491 209 IRChecker();
LtBarbershop 11:521c3e8e6491 210 action = 0;
LtBarbershop 11:521c3e8e6491 211 }
LtBarbershop 11:521c3e8e6491 212 if (action == 3)
LtBarbershop 11:521c3e8e6491 213 {
LtBarbershop 11:521c3e8e6491 214 // display debug messages
LtBarbershop 11:521c3e8e6491 215
LtBarbershop 11:521c3e8e6491 216 //Var_Lock.lock();
LtBarbershop 11:521c3e8e6491 217 BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
LtBarbershop 11:521c3e8e6491 218 BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
LtBarbershop 11:521c3e8e6491 219 BtS.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
LtBarbershop 11:521c3e8e6491 220 BtS.printf("e L: %f R: %f \r\n", eL, eR);
LtBarbershop 11:521c3e8e6491 221 BtS.printf("Ae L: %f R: %f \n\r", aeL, aeR);
LtBarbershop 11:521c3e8e6491 222 BtS.printf("cSP L: %f R: %f \r\n\n", cSetL, cSetR);
LtBarbershop 11:521c3e8e6491 223 //Var_Lock.unlock();
LtBarbershop 11:521c3e8e6491 224
LtBarbershop 11:521c3e8e6491 225 Thread::wait(2000);
LtBarbershop 11:521c3e8e6491 226 }
LtBarbershop 11:521c3e8e6491 227
LtBarbershop 11:521c3e8e6491 228
LtBarbershop 11:521c3e8e6491 229 /*
IanTheMBEDMaster 2:1c5cc180812d 230 InitializeSystem();
LtBarbershop 10:a3ecedc8d9d7 231 BTInit();
LtBarbershop 10:a3ecedc8d9d7 232 BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
IanTheMBEDMaster 2:1c5cc180812d 233
LtBarbershop 10:a3ecedc8d9d7 234 BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
LtBarbershop 10:a3ecedc8d9d7 235 char x;
IanTheMBEDMaster 2:1c5cc180812d 236 while(1)
IanTheMBEDMaster 2:1c5cc180812d 237 {
LtBarbershop 10:a3ecedc8d9d7 238 Thread::wait(20);
LtBarbershop 10:a3ecedc8d9d7 239
LtBarbershop 10:a3ecedc8d9d7 240 if (BtS.readable())
LtBarbershop 10:a3ecedc8d9d7 241 {
LtBarbershop 10:a3ecedc8d9d7 242 x = BtS.getc();
LtBarbershop 10:a3ecedc8d9d7 243
LtBarbershop 10:a3ecedc8d9d7 244 switch(x)
LtBarbershop 10:a3ecedc8d9d7 245 {
LtBarbershop 10:a3ecedc8d9d7 246 case('w'):
LtBarbershop 10:a3ecedc8d9d7 247 userSetL = userSetL + 0.05;
LtBarbershop 10:a3ecedc8d9d7 248 userSetR = userSetR + 0.05;
LtBarbershop 10:a3ecedc8d9d7 249 break;
LtBarbershop 10:a3ecedc8d9d7 250 case('s'):
LtBarbershop 10:a3ecedc8d9d7 251 userSetL = userSetL - 0.05;
LtBarbershop 10:a3ecedc8d9d7 252 userSetR = userSetR - 0.05;
LtBarbershop 10:a3ecedc8d9d7 253 break;
LtBarbershop 10:a3ecedc8d9d7 254 case('a'):
LtBarbershop 10:a3ecedc8d9d7 255 userSetL = userSetL - 0.025;
LtBarbershop 10:a3ecedc8d9d7 256 userSetR = userSetR + 0.025;
LtBarbershop 10:a3ecedc8d9d7 257 break;
LtBarbershop 10:a3ecedc8d9d7 258 case('d'):
LtBarbershop 10:a3ecedc8d9d7 259 userSetL = userSetL + 0.025;
LtBarbershop 10:a3ecedc8d9d7 260 userSetR = userSetR - 0.025;
LtBarbershop 10:a3ecedc8d9d7 261 break;
LtBarbershop 10:a3ecedc8d9d7 262 case('b'):
LtBarbershop 10:a3ecedc8d9d7 263 // Eventually ramp down to 0, RampDown();
LtBarbershop 10:a3ecedc8d9d7 264 userSetL = 0;
LtBarbershop 10:a3ecedc8d9d7 265 userSetR = 0;
LtBarbershop 10:a3ecedc8d9d7 266 break;
LtBarbershop 10:a3ecedc8d9d7 267 }
LtBarbershop 10:a3ecedc8d9d7 268
LtBarbershop 10:a3ecedc8d9d7 269 }
LtBarbershop 10:a3ecedc8d9d7 270
LtBarbershop 10:a3ecedc8d9d7 271 /*
LtBarbershop 8:d65cba3bfc0e 272 char c;
LtBarbershop 8:d65cba3bfc0e 273
IanTheMBEDMaster 7:751d5e3e9cab 274 //Var_Lock.lock();
LtBarbershop 11:521c3e8e6491 275 BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
LtBarbershop 11:521c3e8e6491 276 BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
LtBarbershop 11:521c3e8e6491 277 BtS.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
LtBarbershop 11:521c3e8e6491 278 BtS.printf("e L: %f R: %f \r\n", eL, eR);
LtBarbershop 11:521c3e8e6491 279 BtS.printf("Ae L: %f R: %f \n\r", aeL, aeR);
LtBarbershop 11:521c3e8e6491 280 BtS.printf("cSP L: %f R: %f \r\n\n", cSetL, cSetR);
IanTheMBEDMaster 7:751d5e3e9cab 281 //Var_Lock.unlock();
LtBarbershop 4:03bf5bdca9fb 282
LtBarbershop 11:521c3e8e6491 283 if (BtS.readable()){
LtBarbershop 11:521c3e8e6491 284 x=BtS.getc();
LtBarbershop 11:521c3e8e6491 285 BtS.putc(x); //Echo keyboard entry
IanTheMBEDMaster 2:1c5cc180812d 286 osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
IanTheMBEDMaster 2:1c5cc180812d 287
LtBarbershop 10:a3ecedc8d9d7 288 }
LtBarbershop 11:521c3e8e6491 289 if(BtS.readable())
LtBarbershop 1:3a40c918ff41 290 {
LtBarbershop 11:521c3e8e6491 291 c = BtS.getc();
LtBarbershop 8:d65cba3bfc0e 292 if (c == 'r')
LtBarbershop 8:d65cba3bfc0e 293 {
LtBarbershop 8:d65cba3bfc0e 294 userSetL = 0.2;
LtBarbershop 8:d65cba3bfc0e 295 }
LtBarbershop 8:d65cba3bfc0e 296 if (c == 'p')
LtBarbershop 8:d65cba3bfc0e 297 {
LtBarbershop 8:d65cba3bfc0e 298 if (RecCount == 100)
LtBarbershop 8:d65cba3bfc0e 299 {
LtBarbershop 11:521c3e8e6491 300 BtS.printf("\n\n\rRecV RecU RecX RecE \n\r");
LtBarbershop 8:d65cba3bfc0e 301 int i = 0;
LtBarbershop 8:d65cba3bfc0e 302 for (i = 0; i < 100; i++)
LtBarbershop 8:d65cba3bfc0e 303 {
LtBarbershop 11:521c3e8e6491 304 BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
LtBarbershop 8:d65cba3bfc0e 305 }
LtBarbershop 8:d65cba3bfc0e 306 }
LtBarbershop 8:d65cba3bfc0e 307
LtBarbershop 8:d65cba3bfc0e 308 }
LtBarbershop 8:d65cba3bfc0e 309
LtBarbershop 8:d65cba3bfc0e 310 if (c == 'n')
LtBarbershop 8:d65cba3bfc0e 311 {
LtBarbershop 11:521c3e8e6491 312 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 11:521c3e8e6491 313 BtS.scanf("%f", &userSetL);
LtBarbershop 8:d65cba3bfc0e 314
LtBarbershop 11:521c3e8e6491 315 BtS.printf("%f", userSetL);
LtBarbershop 8:d65cba3bfc0e 316 userSetR = userSetL;
LtBarbershop 8:d65cba3bfc0e 317 }
IanTheMBEDMaster 2:1c5cc180812d 318 }
IanTheMBEDMaster 2:1c5cc180812d 319
LtBarbershop 10:a3ecedc8d9d7 320 Thread::wait(2000); Wait 2 seconds */
LtBarbershop 1:3a40c918ff41 321 }
LtBarbershop 1:3a40c918ff41 322 }
LtBarbershop 1:3a40c918ff41 323
LtBarbershop 1:3a40c918ff41 324 // ******** Control Thread ********
IanTheMBEDMaster 2:1c5cc180812d 325 void PiControlThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 326 {
IanTheMBEDMaster 2:1c5cc180812d 327 while (1)
IanTheMBEDMaster 2:1c5cc180812d 328 {
IanTheMBEDMaster 2:1c5cc180812d 329 osSignalWait(SignalPi, osWaitForever);
IanTheMBEDMaster 2:1c5cc180812d 330 led2= !led2; // Alive status
IanTheMBEDMaster 2:1c5cc180812d 331
IanTheMBEDMaster 6:5200ce9fa5a7 332 float prevu1, prevu2;
LtBarbershop 5:7108ac9e8182 333 //float eL = 0;
LtBarbershop 5:7108ac9e8182 334 //float eR = 0;
IanTheMBEDMaster 6:5200ce9fa5a7 335 const unsigned short maxError = 1000;
IanTheMBEDMaster 6:5200ce9fa5a7 336 const unsigned short maxAcc = 10000;
LtBarbershop 8:d65cba3bfc0e 337 // Kp = 0.1, Ki = 0.5
IanTheMBEDMaster 7:751d5e3e9cab 338 const float Kp = 0.1f;
LtBarbershop 8:d65cba3bfc0e 339 const float Ki = 0.5f;
LtBarbershop 5:7108ac9e8182 340
LtBarbershop 5:7108ac9e8182 341 prevu1 = u1;
LtBarbershop 5:7108ac9e8182 342 prevu2 = u2;
LtBarbershop 4:03bf5bdca9fb 343
IanTheMBEDMaster 6:5200ce9fa5a7 344 // read encoder and calculate speed of both motors
IanTheMBEDMaster 7:751d5e3e9cab 345 GetSpeeds();
LtBarbershop 4:03bf5bdca9fb 346
LtBarbershop 4:03bf5bdca9fb 347 // calculate error
LtBarbershop 5:7108ac9e8182 348 eL = userSetL - fbSpeedL;
LtBarbershop 5:7108ac9e8182 349 eR = userSetR - fbSpeedR;
LtBarbershop 5:7108ac9e8182 350 //eL = -eL;
LtBarbershop 5:7108ac9e8182 351 //eR = -eR;
IanTheMBEDMaster 6:5200ce9fa5a7 352
LtBarbershop 5:7108ac9e8182 353 // prevent overflow / bound the error
LtBarbershop 5:7108ac9e8182 354 /*
LtBarbershop 5:7108ac9e8182 355 if (eL > maxError)
LtBarbershop 5:7108ac9e8182 356 {
LtBarbershop 5:7108ac9e8182 357 eL = maxError;
LtBarbershop 5:7108ac9e8182 358 }
LtBarbershop 5:7108ac9e8182 359 if (eL < -maxError);
LtBarbershop 5:7108ac9e8182 360 {
LtBarbershop 5:7108ac9e8182 361 eL = -maxError;
LtBarbershop 5:7108ac9e8182 362 }
LtBarbershop 5:7108ac9e8182 363 if (eR > maxError)
LtBarbershop 5:7108ac9e8182 364 {
LtBarbershop 5:7108ac9e8182 365 eR = maxError;
LtBarbershop 5:7108ac9e8182 366 }
LtBarbershop 5:7108ac9e8182 367 if (eR < -maxError);
LtBarbershop 5:7108ac9e8182 368 {
LtBarbershop 5:7108ac9e8182 369 eR = -maxError;
LtBarbershop 5:7108ac9e8182 370 }
IanTheMBEDMaster 7:751d5e3e9cab 371 */
LtBarbershop 4:03bf5bdca9fb 372
LtBarbershop 4:03bf5bdca9fb 373 // accumulated error (integration)
LtBarbershop 5:7108ac9e8182 374 if (prevu1 < 1 && prevu1 > -1)
LtBarbershop 5:7108ac9e8182 375 {
LtBarbershop 5:7108ac9e8182 376 aeL += eL;
LtBarbershop 5:7108ac9e8182 377 }
LtBarbershop 5:7108ac9e8182 378 if (prevu2 < 1 && prevu2 > -1)
LtBarbershop 5:7108ac9e8182 379 {
LtBarbershop 5:7108ac9e8182 380 aeR += eR;
LtBarbershop 5:7108ac9e8182 381 }
LtBarbershop 5:7108ac9e8182 382
LtBarbershop 5:7108ac9e8182 383 // bound the accumulatd error
LtBarbershop 5:7108ac9e8182 384 /*
LtBarbershop 5:7108ac9e8182 385 if (aeL > maxAcc)
LtBarbershop 5:7108ac9e8182 386 {
LtBarbershop 5:7108ac9e8182 387 aeL = maxAcc;
LtBarbershop 5:7108ac9e8182 388 }
LtBarbershop 5:7108ac9e8182 389 if (aeL < -maxAcc)
LtBarbershop 5:7108ac9e8182 390 {
LtBarbershop 5:7108ac9e8182 391 aeL = -maxAcc;
LtBarbershop 5:7108ac9e8182 392 }
LtBarbershop 5:7108ac9e8182 393 if (aeR > maxAcc)
LtBarbershop 5:7108ac9e8182 394 {
LtBarbershop 5:7108ac9e8182 395 aeR = maxAcc;
LtBarbershop 5:7108ac9e8182 396 }
LtBarbershop 5:7108ac9e8182 397 if (aeR < -maxAcc)
LtBarbershop 5:7108ac9e8182 398 {
LtBarbershop 5:7108ac9e8182 399 aeR = -maxAcc;
LtBarbershop 5:7108ac9e8182 400 }
LtBarbershop 5:7108ac9e8182 401 */
LtBarbershop 4:03bf5bdca9fb 402
LtBarbershop 4:03bf5bdca9fb 403 u1 = Kp*eL + Ki*aeL;
LtBarbershop 4:03bf5bdca9fb 404 u2 = Kp*eR + Ki*aeR;
LtBarbershop 5:7108ac9e8182 405
IanTheMBEDMaster 7:751d5e3e9cab 406 cSetL = userSetL + u1;
IanTheMBEDMaster 7:751d5e3e9cab 407 cSetR = userSetR + u2;
LtBarbershop 5:7108ac9e8182 408
IanTheMBEDMaster 7:751d5e3e9cab 409 //u1 = -u1;
IanTheMBEDMaster 7:751d5e3e9cab 410 //u2 = -u2;
LtBarbershop 4:03bf5bdca9fb 411 // Is signaled by a periodic timer interrupt handler
LtBarbershop 4:03bf5bdca9fb 412 /*
LtBarbershop 4:03bf5bdca9fb 413 Read incremental position, dPosition, and time interval from the QEI.
LtBarbershop 4:03bf5bdca9fb 414 e = Setpoint – dPosition // e is the velocity error
LtBarbershop 4:03bf5bdca9fb 415 xState = xState + e; // x is the Euler approximation to the integral of e.
LtBarbershop 4:03bf5bdca9fb 416 u = Kp*e + Ki*xState; // u is the control signal
LtBarbershop 4:03bf5bdca9fb 417 Update PWM on-time register with abs(u);
LtBarbershop 4:03bf5bdca9fb 418 Update the DIR pin on the LMD18200 with the sign of u.
LtBarbershop 4:03bf5bdca9fb 419 */
LtBarbershop 11:521c3e8e6491 420 /*if (userSetL == 0.8f)
LtBarbershop 8:d65cba3bfc0e 421 {
LtBarbershop 8:d65cba3bfc0e 422 if (RecCount < 100)
LtBarbershop 8:d65cba3bfc0e 423 {
LtBarbershop 8:d65cba3bfc0e 424 RecX[RecCount] = aeL;
LtBarbershop 8:d65cba3bfc0e 425 RecU[RecCount] = cSetL;
LtBarbershop 8:d65cba3bfc0e 426 RecV[RecCount] = fbSpeedL;
LtBarbershop 8:d65cba3bfc0e 427 RecE[RecCount] = eL;
LtBarbershop 8:d65cba3bfc0e 428 RecCount++;
LtBarbershop 8:d65cba3bfc0e 429 }
LtBarbershop 8:d65cba3bfc0e 430 else
LtBarbershop 8:d65cba3bfc0e 431 {
LtBarbershop 8:d65cba3bfc0e 432 userSetL = 0;
LtBarbershop 8:d65cba3bfc0e 433 }
LtBarbershop 11:521c3e8e6491 434
LtBarbershop 11:521c3e8e6491 435 }*/
IanTheMBEDMaster 7:751d5e3e9cab 436 SetLeftMotorSpeed(cSetL);
IanTheMBEDMaster 7:751d5e3e9cab 437 SetRightMotorSpeed(cSetR);
IanTheMBEDMaster 2:1c5cc180812d 438 }
IanTheMBEDMaster 2:1c5cc180812d 439 }
LtBarbershop 1:3a40c918ff41 440
LtBarbershop 1:3a40c918ff41 441 // ******** Collision Thread ********
IanTheMBEDMaster 2:1c5cc180812d 442 void ExtCollisionThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 443 {
IanTheMBEDMaster 2:1c5cc180812d 444 while (1)
IanTheMBEDMaster 2:1c5cc180812d 445 {
IanTheMBEDMaster 2:1c5cc180812d 446 osSignalWait(SignalExtCollision, osWaitForever);
IanTheMBEDMaster 2:1c5cc180812d 447 led4 = !led4;
LtBarbershop 1:3a40c918ff41 448 }
LtBarbershop 1:3a40c918ff41 449 }
LtBarbershop 1:3a40c918ff41 450
LtBarbershop 1:3a40c918ff41 451 // ******** Watchdog Interrupt Handler ********
IanTheMBEDMaster 2:1c5cc180812d 452 void Watchdog(void const *n)
IanTheMBEDMaster 2:1c5cc180812d 453 {
LtBarbershop 1:3a40c918ff41 454 led3=1;
LtBarbershop 11:521c3e8e6491 455 BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
LtBarbershop 1:3a40c918ff41 456 }
LtBarbershop 1:3a40c918ff41 457
LtBarbershop 1:3a40c918ff41 458 // ******** Period Timer Interrupt Handler ********
LtBarbershop 5:7108ac9e8182 459 void PiControllerISR(void)
IanTheMBEDMaster 2:1c5cc180812d 460 {
LtBarbershop 1:3a40c918ff41 461 osSignalSet(PiControl,0x1);
IanTheMBEDMaster 2:1c5cc180812d 462 }
LtBarbershop 1:3a40c918ff41 463
LtBarbershop 1:3a40c918ff41 464 // ******** Collision Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 465 void ExtCollisionISR(void)
LtBarbershop 1:3a40c918ff41 466 {
LtBarbershop 1:3a40c918ff41 467 osSignalSet(ExtCollision,0x1);
LtBarbershop 1:3a40c918ff41 468 }
LtBarbershop 1:3a40c918ff41 469
IanTheMBEDMaster 2:1c5cc180812d 470 // --- Initialization Functions
IanTheMBEDMaster 2:1c5cc180812d 471 void InitializeSystem()
IanTheMBEDMaster 2:1c5cc180812d 472 {
IanTheMBEDMaster 2:1c5cc180812d 473 led3=0;
IanTheMBEDMaster 2:1c5cc180812d 474 led4=0;
IanTheMBEDMaster 2:1c5cc180812d 475
IanTheMBEDMaster 2:1c5cc180812d 476 Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
IanTheMBEDMaster 2:1c5cc180812d 477
IanTheMBEDMaster 2:1c5cc180812d 478 // Start execution of the Threads
IanTheMBEDMaster 2:1c5cc180812d 479 PiControl = osThreadCreate(osThread(PiControlThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 480 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 481 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
LtBarbershop 4:03bf5bdca9fb 482 PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
IanTheMBEDMaster 2:1c5cc180812d 483
IanTheMBEDMaster 6:5200ce9fa5a7 484 InitializePWM();
IanTheMBEDMaster 2:1c5cc180812d 485 InitializeEncoder();
IanTheMBEDMaster 2:1c5cc180812d 486 }
IanTheMBEDMaster 2:1c5cc180812d 487
IanTheMBEDMaster 2:1c5cc180812d 488 void InitializePWM()
IanTheMBEDMaster 2:1c5cc180812d 489 {
IanTheMBEDMaster 6:5200ce9fa5a7 490 PwmL.period(PWMPeriod);
IanTheMBEDMaster 6:5200ce9fa5a7 491 PwmR.period(PWMPeriod);
IanTheMBEDMaster 2:1c5cc180812d 492 }
IanTheMBEDMaster 2:1c5cc180812d 493
IanTheMBEDMaster 2:1c5cc180812d 494 void InitializeEncoder()
IanTheMBEDMaster 2:1c5cc180812d 495 {
IanTheMBEDMaster 2:1c5cc180812d 496 // Initialization – to be executed once (normally)
IanTheMBEDMaster 2:1c5cc180812d 497 DE0.format(16,0); // SPI format: 16-bit words, mode 0 protocol.
LtBarbershop 4:03bf5bdca9fb 498 DE0.frequency(1000000);
IanTheMBEDMaster 2:1c5cc180812d 499 SpiStart = 0;
IanTheMBEDMaster 2:1c5cc180812d 500 SpiReset = 1;
IanTheMBEDMaster 2:1c5cc180812d 501 wait_us(10);
IanTheMBEDMaster 2:1c5cc180812d 502 SpiReset = 0;
IanTheMBEDMaster 2:1c5cc180812d 503 DE0.write(0x8004); // SPI slave control word to read (only) 4-word transactions
IanTheMBEDMaster 2:1c5cc180812d 504 // starting at base address 0 within the peripheral.
IanTheMBEDMaster 2:1c5cc180812d 505 }
IanTheMBEDMaster 2:1c5cc180812d 506
IanTheMBEDMaster 2:1c5cc180812d 507
IanTheMBEDMaster 2:1c5cc180812d 508 // --- Other Functions
IanTheMBEDMaster 2:1c5cc180812d 509 void SetLeftMotorSpeed(float u)
LtBarbershop 1:3a40c918ff41 510 {
IanTheMBEDMaster 2:1c5cc180812d 511 float T;
IanTheMBEDMaster 2:1c5cc180812d 512 float d;
IanTheMBEDMaster 2:1c5cc180812d 513 float onTime;
IanTheMBEDMaster 2:1c5cc180812d 514
LtBarbershop 3:9a39e487b724 515 // bound the input
LtBarbershop 3:9a39e487b724 516 if (u > 1)
LtBarbershop 3:9a39e487b724 517 {
LtBarbershop 3:9a39e487b724 518 u = 1;
LtBarbershop 3:9a39e487b724 519 }
LtBarbershop 3:9a39e487b724 520
LtBarbershop 3:9a39e487b724 521 if (u < -1)
LtBarbershop 3:9a39e487b724 522 {
LtBarbershop 3:9a39e487b724 523 u = -1;
LtBarbershop 3:9a39e487b724 524 }
LtBarbershop 3:9a39e487b724 525
LtBarbershop 3:9a39e487b724 526 // calculate duty cycle timing
IanTheMBEDMaster 2:1c5cc180812d 527 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 528 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 529 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 530
IanTheMBEDMaster 2:1c5cc180812d 531 PwmL.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 532
IanTheMBEDMaster 2:1c5cc180812d 533 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 534 {
IanTheMBEDMaster 2:1c5cc180812d 535 dirL = 1;
IanTheMBEDMaster 2:1c5cc180812d 536 }
IanTheMBEDMaster 2:1c5cc180812d 537 else
IanTheMBEDMaster 2:1c5cc180812d 538 {
IanTheMBEDMaster 2:1c5cc180812d 539 dirL = 0;
IanTheMBEDMaster 2:1c5cc180812d 540 }
IanTheMBEDMaster 2:1c5cc180812d 541 }
LtBarbershop 1:3a40c918ff41 542
IanTheMBEDMaster 2:1c5cc180812d 543 void SetRightMotorSpeed(float u)
IanTheMBEDMaster 2:1c5cc180812d 544 {
IanTheMBEDMaster 2:1c5cc180812d 545 float T;
IanTheMBEDMaster 2:1c5cc180812d 546 float d;
IanTheMBEDMaster 2:1c5cc180812d 547 float onTime;
IanTheMBEDMaster 2:1c5cc180812d 548
LtBarbershop 3:9a39e487b724 549 // bound the input
LtBarbershop 3:9a39e487b724 550 if (u > 1)
LtBarbershop 3:9a39e487b724 551 {
LtBarbershop 3:9a39e487b724 552 u = 1;
LtBarbershop 3:9a39e487b724 553 }
LtBarbershop 3:9a39e487b724 554
LtBarbershop 3:9a39e487b724 555 if (u < -1)
LtBarbershop 3:9a39e487b724 556 {
LtBarbershop 3:9a39e487b724 557 u = -1;
LtBarbershop 3:9a39e487b724 558 }
LtBarbershop 3:9a39e487b724 559
LtBarbershop 3:9a39e487b724 560 // calculate duty cycle timing
IanTheMBEDMaster 2:1c5cc180812d 561 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 562 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 563 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 564
IanTheMBEDMaster 2:1c5cc180812d 565 PwmR.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 566
IanTheMBEDMaster 2:1c5cc180812d 567 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 568 {
IanTheMBEDMaster 2:1c5cc180812d 569 dirR = 1;
IanTheMBEDMaster 2:1c5cc180812d 570 }
IanTheMBEDMaster 2:1c5cc180812d 571 else
IanTheMBEDMaster 2:1c5cc180812d 572 {
IanTheMBEDMaster 2:1c5cc180812d 573 dirR = 0;
IanTheMBEDMaster 2:1c5cc180812d 574 }
LtBarbershop 1:3a40c918ff41 575 }
LtBarbershop 1:3a40c918ff41 576
IanTheMBEDMaster 7:751d5e3e9cab 577 void GetSpeeds()
LtBarbershop 1:3a40c918ff41 578 {
IanTheMBEDMaster 7:751d5e3e9cab 579 float leftMaxPos = 1480.0f;
IanTheMBEDMaster 7:751d5e3e9cab 580 float rightMaxPos = 1480.0f;
LtBarbershop 1:3a40c918ff41 581
IanTheMBEDMaster 6:5200ce9fa5a7 582 // Restart the SPI module each time
LtBarbershop 4:03bf5bdca9fb 583 SpiStart = 1;
LtBarbershop 4:03bf5bdca9fb 584 wait_us(5);
LtBarbershop 4:03bf5bdca9fb 585 SpiStart = 0;
LtBarbershop 4:03bf5bdca9fb 586 DE0.write(0x8004);
IanTheMBEDMaster 6:5200ce9fa5a7 587 // read in 4 16-bit words
LtBarbershop 4:03bf5bdca9fb 588 Var_Lock.lock();
IanTheMBEDMaster 6:5200ce9fa5a7 589 dPositionLeft = DE0.write(Dummy); // Read QEI-0 position register
IanTheMBEDMaster 6:5200ce9fa5a7 590 dTimeLeft = DE0.write(Dummy); // Read QE-0 time interval register
LtBarbershop 4:03bf5bdca9fb 591 dPositionRight = DE0.write(Dummy); // Read QEI-1 position register
LtBarbershop 4:03bf5bdca9fb 592 dTimeRight = DE0.write(Dummy); // Read QEI-1 time interval register
LtBarbershop 4:03bf5bdca9fb 593
IanTheMBEDMaster 6:5200ce9fa5a7 594 // figure out which direction the motor is turning
IanTheMBEDMaster 7:751d5e3e9cab 595 /*
IanTheMBEDMaster 6:5200ce9fa5a7 596 if (dPositionLeft > 32767)
LtBarbershop 4:03bf5bdca9fb 597 {
IanTheMBEDMaster 6:5200ce9fa5a7 598 // turning backwards
IanTheMBEDMaster 6:5200ce9fa5a7 599 *leftSpeed = -(65535 - dPositionLeft)/leftMaxPos;
LtBarbershop 4:03bf5bdca9fb 600 }
LtBarbershop 4:03bf5bdca9fb 601 else
LtBarbershop 4:03bf5bdca9fb 602 {
IanTheMBEDMaster 6:5200ce9fa5a7 603 // turning forwards
IanTheMBEDMaster 6:5200ce9fa5a7 604 *leftSpeed = dPositionLeft/leftMaxPos;
LtBarbershop 4:03bf5bdca9fb 605 }
IanTheMBEDMaster 6:5200ce9fa5a7 606
IanTheMBEDMaster 6:5200ce9fa5a7 607 if (dPositionRight > 32767)
IanTheMBEDMaster 6:5200ce9fa5a7 608 {
IanTheMBEDMaster 6:5200ce9fa5a7 609 // turning backwards
IanTheMBEDMaster 6:5200ce9fa5a7 610 *rightSpeed = -(65535 - dPositionRight)/rightMaxPos;
IanTheMBEDMaster 6:5200ce9fa5a7 611 }
IanTheMBEDMaster 6:5200ce9fa5a7 612 else
IanTheMBEDMaster 6:5200ce9fa5a7 613 {
IanTheMBEDMaster 6:5200ce9fa5a7 614 // turning forwards
IanTheMBEDMaster 6:5200ce9fa5a7 615 *rightSpeed = dPositionRight/rightMaxPos;
IanTheMBEDMaster 6:5200ce9fa5a7 616 }
IanTheMBEDMaster 7:751d5e3e9cab 617 */
IanTheMBEDMaster 6:5200ce9fa5a7 618 Var_Lock.unlock();
LtBarbershop 4:03bf5bdca9fb 619
IanTheMBEDMaster 7:751d5e3e9cab 620 // calcspeed
IanTheMBEDMaster 7:751d5e3e9cab 621 fbSpeedL = ((float)dPositionLeft)/leftMaxPos;
IanTheMBEDMaster 7:751d5e3e9cab 622 fbSpeedR = ((float)dPositionRight)/rightMaxPos;
IanTheMBEDMaster 7:751d5e3e9cab 623
IanTheMBEDMaster 6:5200ce9fa5a7 624 // bound the feedback speed
IanTheMBEDMaster 7:751d5e3e9cab 625 if (fbSpeedL > 1)
IanTheMBEDMaster 6:5200ce9fa5a7 626 {
IanTheMBEDMaster 7:751d5e3e9cab 627 fbSpeedL = 1;
IanTheMBEDMaster 6:5200ce9fa5a7 628 }
IanTheMBEDMaster 7:751d5e3e9cab 629 if (fbSpeedL < -1)
IanTheMBEDMaster 6:5200ce9fa5a7 630 {
IanTheMBEDMaster 7:751d5e3e9cab 631 fbSpeedL = -1;
IanTheMBEDMaster 6:5200ce9fa5a7 632 }
IanTheMBEDMaster 7:751d5e3e9cab 633 if (fbSpeedR > 1)
IanTheMBEDMaster 6:5200ce9fa5a7 634 {
IanTheMBEDMaster 7:751d5e3e9cab 635 fbSpeedR = 1;
IanTheMBEDMaster 6:5200ce9fa5a7 636 }
IanTheMBEDMaster 7:751d5e3e9cab 637 if (fbSpeedR < -1)
IanTheMBEDMaster 6:5200ce9fa5a7 638 {
IanTheMBEDMaster 7:751d5e3e9cab 639 fbSpeedR = -1;
IanTheMBEDMaster 6:5200ce9fa5a7 640 }
LtBarbershop 9:0eb7b173d6c3 641 }
LtBarbershop 11:521c3e8e6491 642
LtBarbershop 11:521c3e8e6491 643 void IRChecker()
LtBarbershop 9:0eb7b173d6c3 644 {
LtBarbershop 9:0eb7b173d6c3 645 float IRF, IRR;
LtBarbershop 11:521c3e8e6491 646 char z;
LtBarbershop 9:0eb7b173d6c3 647 // Read Sensors
LtBarbershop 11:521c3e8e6491 648 while (z != 'q')
LtBarbershop 11:521c3e8e6491 649 {
LtBarbershop 11:521c3e8e6491 650 IRF = IRFront.read();
LtBarbershop 11:521c3e8e6491 651 IRR = IRRear.read();
LtBarbershop 11:521c3e8e6491 652 // Voltage Corresponding to nominal distance
LtBarbershop 11:521c3e8e6491 653 BtS.printf("\n\rFront Sensor: %f", IRF);
LtBarbershop 11:521c3e8e6491 654 BtS.printf("\n\rRear Sensor: %f\n\r", IRR);
LtBarbershop 11:521c3e8e6491 655 if (BtS.readable())
LtBarbershop 11:521c3e8e6491 656 {
LtBarbershop 11:521c3e8e6491 657 z = BtS.getc();
LtBarbershop 11:521c3e8e6491 658 }
LtBarbershop 11:521c3e8e6491 659 Thread::wait(1000);
LtBarbershop 11:521c3e8e6491 660 }
LtBarbershop 9:0eb7b173d6c3 661 float Nominal = 3.0;
LtBarbershop 9:0eb7b173d6c3 662 // Variable for turning robots
LtBarbershop 9:0eb7b173d6c3 663 float Turn = 0.1;
LtBarbershop 9:0eb7b173d6c3 664
LtBarbershop 9:0eb7b173d6c3 665 // Ensure Robot isn't closer than 10cm from wall (reads no voltage)
LtBarbershop 9:0eb7b173d6c3 666
LtBarbershop 11:521c3e8e6491 667 /* Compare to nominal voltage and adjust
LtBarbershop 9:0eb7b173d6c3 668 if (IRF > Nominal && IRR > Nominal)
LtBarbershop 9:0eb7b173d6c3 669 {
LtBarbershop 9:0eb7b173d6c3 670 SetR = SetR - Turn;
LtBarbershop 9:0eb7b173d6c3 671 SetL = SetL + Turn;
LtBarbershop 9:0eb7b173d6c3 672 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 673 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 674 }
LtBarbershop 9:0eb7b173d6c3 675 if (IRF < Nominal && IRR < Nominal
LtBarbershop 9:0eb7b173d6c3 676 {
LtBarbershop 9:0eb7b173d6c3 677 SetR = SetR - Turn;
LtBarbershop 9:0eb7b173d6c3 678 SetL = SetL + Turn;
LtBarbershop 9:0eb7b173d6c3 679 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 680 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 681 }
LtBarbershop 9:0eb7b173d6c3 682
LtBarbershop 9:0eb7b173d6c3 683 // Compare rangers to each other
LtBarbershop 9:0eb7b173d6c3 684 if (IRF > IRR) // IRF closer than IRR
LtBarbershop 9:0eb7b173d6c3 685 {
LtBarbershop 9:0eb7b173d6c3 686 SetR = SetR + Turn;
LtBarbershop 9:0eb7b173d6c3 687 SetL = SetL - Turn;
LtBarbershop 9:0eb7b173d6c3 688 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 689 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 690 }
LtBarbershop 9:0eb7b173d6c3 691 if (IRF < IRR) // IRF further than IRR
LtBarbershop 9:0eb7b173d6c3 692 {
LtBarbershop 9:0eb7b173d6c3 693 SetR = SetR - Turn;
LtBarbershop 9:0eb7b173d6c3 694 SetL = SetL + Turn;
LtBarbershop 9:0eb7b173d6c3 695 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 696 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 697 }
LtBarbershop 11:521c3e8e6491 698 */
LtBarbershop 11:521c3e8e6491 699 }
LtBarbershop 10:a3ecedc8d9d7 700
LtBarbershop 10:a3ecedc8d9d7 701 void BTInit()
LtBarbershop 10:a3ecedc8d9d7 702 {
LtBarbershop 10:a3ecedc8d9d7 703 BtS.printf("AT+BTCANCEL\r\n");
LtBarbershop 10:a3ecedc8d9d7 704 BtS.printf("AT+BTSCAN\r\n");
LtBarbershop 11:521c3e8e6491 705 }
LtBarbershop 11:521c3e8e6491 706
LtBarbershop 11:521c3e8e6491 707 void DisplayMenu()
LtBarbershop 11:521c3e8e6491 708 {
LtBarbershop 11:521c3e8e6491 709 BtS.printf("\r\n --- Robot Initialization Complete --- \r\n\n");
LtBarbershop 11:521c3e8e6491 710 BtS.printf(" Press the corresponding key to do something:\r\n");
LtBarbershop 11:521c3e8e6491 711 BtS.printf("| Key | Action\n\r");
LtBarbershop 11:521c3e8e6491 712 BtS.printf("|-----|-------\n\r");
LtBarbershop 11:521c3e8e6491 713 BtS.printf("| d | Drive the robot using wasd keys\n\r");
LtBarbershop 11:521c3e8e6491 714 BtS.printf("| w | Robot performs wall following\n\r");
LtBarbershop 11:521c3e8e6491 715 BtS.printf("| 0 | Debug mode\n\r");
LtBarbershop 11:521c3e8e6491 716 BtS.printf("| q | Quit current action, stop the robot, and return to this menu\n\r\n");
LtBarbershop 11:521c3e8e6491 717
LtBarbershop 11:521c3e8e6491 718 }
LtBarbershop 11:521c3e8e6491 719
LtBarbershop 11:521c3e8e6491 720 void Ramp(float speed, unsigned int time, unsigned short motor)
LtBarbershop 11:521c3e8e6491 721 {
LtBarbershop 11:521c3e8e6491 722 const unsigned short steps = 20;
LtBarbershop 11:521c3e8e6491 723 float changeL = (speed - userSetL)/steps;
LtBarbershop 11:521c3e8e6491 724 float changeR = (speed - userSetR)/steps;
LtBarbershop 11:521c3e8e6491 725 unsigned short i;
LtBarbershop 11:521c3e8e6491 726 // calculate wait time (we wont worry too much about rounding errors)
LtBarbershop 11:521c3e8e6491 727 unsigned int waitTime = time/steps;
LtBarbershop 11:521c3e8e6491 728
LtBarbershop 11:521c3e8e6491 729 for (i = 0; i < steps; i++)
LtBarbershop 11:521c3e8e6491 730 {
LtBarbershop 11:521c3e8e6491 731 //Thread::wait(200);
LtBarbershop 11:521c3e8e6491 732 Thread::wait(waitTime);
LtBarbershop 11:521c3e8e6491 733
LtBarbershop 11:521c3e8e6491 734 if (motor == 0)
LtBarbershop 11:521c3e8e6491 735 {
LtBarbershop 11:521c3e8e6491 736 // change both speeds
LtBarbershop 11:521c3e8e6491 737 userSetL += changeL;
LtBarbershop 11:521c3e8e6491 738 userSetR += changeR;
LtBarbershop 11:521c3e8e6491 739 continue;
LtBarbershop 11:521c3e8e6491 740 }
LtBarbershop 11:521c3e8e6491 741 if (motor == 1)
LtBarbershop 11:521c3e8e6491 742 {
LtBarbershop 11:521c3e8e6491 743 userSetL += changeL;
LtBarbershop 11:521c3e8e6491 744 continue;
LtBarbershop 11:521c3e8e6491 745 }
LtBarbershop 11:521c3e8e6491 746 if (motor == 2)
LtBarbershop 11:521c3e8e6491 747 {
LtBarbershop 11:521c3e8e6491 748 userSetR += changeR;
LtBarbershop 11:521c3e8e6491 749 }
LtBarbershop 11:521c3e8e6491 750
LtBarbershop 11:521c3e8e6491 751
LtBarbershop 11:521c3e8e6491 752 }
LtBarbershop 11:521c3e8e6491 753
LtBarbershop 9:0eb7b173d6c3 754 }