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@24:f4f933d9194b, 2013-04-13 (annotated)

Committer:
IanTheMBEDMaster
Date:
Sat Apr 13 21:42:56 2013 +0000
Revision:
24:f4f933d9194b
Parent:
23:806c9b8af77c 
Finished up

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