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@17:bc13550f673b, 2013-03-20 (annotated)

Committer:
IanTheMBEDMaster
Date:
Wed Mar 20 01:42:03 2013 +0000
Revision:
17:bc13550f673b
Parent:
16:cf9519c35510
Child:
18:4b3ad79d1068
Played with PI

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