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@18:4b3ad79d1068, 2013-03-22 (annotated)

Committer:
LtBarbershop
Date:
Fri Mar 22 19:57:55 2013 +0000
Revision:
18:4b3ad79d1068
Parent:
17:bc13550f673b
Child:
19:b9c4952f4acc
tried some stuff, no too much going on

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