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@15:407e4152cf3a, 2013-03-18 (annotated)

Committer:
IanTheMBEDMaster
Date:
Mon Mar 18 14:38:40 2013 +0000
Revision:
15:407e4152cf3a
Parent:
14:669f2f1566b0
Child:
16:cf9519c35510
added max turn radius bounding

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