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@22:3cda0d788452, 2013-04-10 (annotated)

Committer:
IanTheMBEDMaster
Date:
Wed Apr 10 13:16:29 2013 +0000
Revision:
22:3cda0d788452
Parent:
21:c5713aafdc9d
Child:
23:806c9b8af77c
updated some things

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 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;
IanTheMBEDMaster 22:3cda0d788452 64 float Turn = 0.075;
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;
IanTheMBEDMaster 22:3cda0d788452 72 float Kor = 0.005;
IanTheMBEDMaster 22:3cda0d788452 73 float KpW = 0.4;
LtBarbershop 21:c5713aafdc9d 74 float KiW = 0.0;
IanTheMBEDMaster 17:bc13550f673b 75
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 13:aaac0105a486 176 userSetL = 0.1;
LtBarbershop 13:aaac0105a486 177 userSetR = 0.1;
LtBarbershop 13:aaac0105a486 178 TSpeedL = 0.1;
LtBarbershop 13:aaac0105a486 179 TSpeedR = 0.1;
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'):
IanTheMBEDMaster 22:3cda0d788452 204 userSetL = userSetL + 0.08;
IanTheMBEDMaster 22:3cda0d788452 205 userSetR = userSetR + 0.08;
LtBarbershop 11:521c3e8e6491 206 break;
LtBarbershop 11:521c3e8e6491 207 case('s'):
IanTheMBEDMaster 22:3cda0d788452 208 userSetL = userSetL - 0.08;
IanTheMBEDMaster 22:3cda0d788452 209 userSetR = userSetR - 0.08;
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'):
IanTheMBEDMaster 22:3cda0d788452 220 Ramp(0.4, 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 13:aaac0105a486 260 Turn = Turn + 0.005;
LtBarbershop 13:aaac0105a486 261 break;
LtBarbershop 13:aaac0105a486 262 case('d'):
LtBarbershop 13:aaac0105a486 263 Turn = Turn - 0.005;
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;
IanTheMBEDMaster 19:b9c4952f4acc 271 case('n'):
IanTheMBEDMaster 19:b9c4952f4acc 272 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 273 char k;
IanTheMBEDMaster 19:b9c4952f4acc 274 float newConst;
IanTheMBEDMaster 19:b9c4952f4acc 275 while (1)
IanTheMBEDMaster 19:b9c4952f4acc 276 {
IanTheMBEDMaster 19:b9c4952f4acc 277 if (BtS.readable())
IanTheMBEDMaster 19:b9c4952f4acc 278 {
IanTheMBEDMaster 19:b9c4952f4acc 279 k = BtS.getc();
IanTheMBEDMaster 19:b9c4952f4acc 280 if (k == '1')
IanTheMBEDMaster 19:b9c4952f4acc 281 {
IanTheMBEDMaster 19:b9c4952f4acc 282 BtS.scanf("%f", &newConst);
IanTheMBEDMaster 19:b9c4952f4acc 283 KiW = newConst;
IanTheMBEDMaster 19:b9c4952f4acc 284 break;
IanTheMBEDMaster 19:b9c4952f4acc 285 }
IanTheMBEDMaster 19:b9c4952f4acc 286 if (k == '2')
IanTheMBEDMaster 19:b9c4952f4acc 287 {
IanTheMBEDMaster 19:b9c4952f4acc 288 BtS.scanf("%f", &newConst);
IanTheMBEDMaster 19:b9c4952f4acc 289 KpW = newConst;
IanTheMBEDMaster 19:b9c4952f4acc 290 break;
IanTheMBEDMaster 19:b9c4952f4acc 291 }
IanTheMBEDMaster 19:b9c4952f4acc 292 if (k == '3')
IanTheMBEDMaster 19:b9c4952f4acc 293 {
IanTheMBEDMaster 19:b9c4952f4acc 294 BtS.scanf("%f", &newConst);
IanTheMBEDMaster 19:b9c4952f4acc 295 Kor = newConst;
IanTheMBEDMaster 19:b9c4952f4acc 296 break;
IanTheMBEDMaster 19:b9c4952f4acc 297 }
IanTheMBEDMaster 19:b9c4952f4acc 298 if (k == '4')
IanTheMBEDMaster 19:b9c4952f4acc 299 {
IanTheMBEDMaster 19:b9c4952f4acc 300 BtS.scanf("%f", &newConst);
IanTheMBEDMaster 19:b9c4952f4acc 301 Kpos = newConst;
IanTheMBEDMaster 19:b9c4952f4acc 302 break;
IanTheMBEDMaster 19:b9c4952f4acc 303 }
IanTheMBEDMaster 19:b9c4952f4acc 304
IanTheMBEDMaster 19:b9c4952f4acc 305 printf("\n\r Pick a constant ya goof \n\r");
IanTheMBEDMaster 19:b9c4952f4acc 306 }
IanTheMBEDMaster 19:b9c4952f4acc 307 }
LtBarbershop 13:aaac0105a486 308 }
LtBarbershop 11:521c3e8e6491 309 }
LtBarbershop 11:521c3e8e6491 310 if (action == 3)
LtBarbershop 11:521c3e8e6491 311 {
LtBarbershop 11:521c3e8e6491 312 // keyboard input to debug mode
LtBarbershop 11:521c3e8e6491 313 float newSpeed;
LtBarbershop 11:521c3e8e6491 314 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 11:521c3e8e6491 315 BtS.scanf("%f", &newSpeed);
LtBarbershop 11:521c3e8e6491 316
LtBarbershop 11:521c3e8e6491 317 BtS.printf("%f", newSpeed);
IanTheMBEDMaster 17:bc13550f673b 318 Ramp(newSpeed, 20, 0);
LtBarbershop 11:521c3e8e6491 319 //userSetR = userSetL;
IanTheMBEDMaster 12:3058f9fb09eb 320 }
IanTheMBEDMaster 17:bc13550f673b 321
IanTheMBEDMaster 17:bc13550f673b 322 if (action == 4 && c == 'r')
IanTheMBEDMaster 17:bc13550f673b 323 {
IanTheMBEDMaster 17:bc13550f673b 324 if (RecCount == 100)
IanTheMBEDMaster 17:bc13550f673b 325 {
IanTheMBEDMaster 17:bc13550f673b 326 BtS.printf("\n\n\rRecV RecU RecX RecE \n\r");
IanTheMBEDMaster 17:bc13550f673b 327 int i = 0;
IanTheMBEDMaster 17:bc13550f673b 328 for (i = 0; i < 100; i++)
IanTheMBEDMaster 17:bc13550f673b 329 {
IanTheMBEDMaster 17:bc13550f673b 330 BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
IanTheMBEDMaster 17:bc13550f673b 331 }
IanTheMBEDMaster 17:bc13550f673b 332 }
IanTheMBEDMaster 17:bc13550f673b 333 }
LtBarbershop 11:521c3e8e6491 334 }// close if(BtS.readable())
LtBarbershop 11:521c3e8e6491 335 if (action == 2)
LtBarbershop 11:521c3e8e6491 336 {
IanTheMBEDMaster 17:bc13550f673b 337 // Wall Following
IanTheMBEDMaster 17:bc13550f673b 338 //Var_Lock.lock();
LtBarbershop 18:4b3ad79d1068 339 BtS.printf("IR F: %f cm R: %f cm \n\r", DF, DR);
LtBarbershop 18:4b3ad79d1068 340 BtS.printf("Wall Error: %f \n\r", eW);
LtBarbershop 18:4b3ad79d1068 341 BtS.printf("Acc Error: %f \n\r", aeW);
LtBarbershop 18:4b3ad79d1068 342 BtS.printf("Diff. Setpoint: %f \n\r", uW);
LtBarbershop 18:4b3ad79d1068 343 BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
IanTheMBEDMaster 17:bc13550f673b 344 //Var_Lock.unlock();
LtBarbershop 18:4b3ad79d1068 345 Thread::wait(1000);
LtBarbershop 11:521c3e8e6491 346 }
LtBarbershop 13:aaac0105a486 347
LtBarbershop 11:521c3e8e6491 348 if (action == 3)
LtBarbershop 11:521c3e8e6491 349 {
IanTheMBEDMaster 17:bc13550f673b 350 // Debug Mode
LtBarbershop 11:521c3e8e6491 351
IanTheMBEDMaster 12:3058f9fb09eb 352 float IRF, IRR;
IanTheMBEDMaster 12:3058f9fb09eb 353 IRF = IRFront.read();
IanTheMBEDMaster 12:3058f9fb09eb 354 IRR = IRRear.read();
IanTheMBEDMaster 17:bc13550f673b 355
IanTheMBEDMaster 12:3058f9fb09eb 356 Var_Lock.lock();
LtBarbershop 11:521c3e8e6491 357 BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
LtBarbershop 11:521c3e8e6491 358 BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
LtBarbershop 11:521c3e8e6491 359 BtS.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
LtBarbershop 11:521c3e8e6491 360 BtS.printf("e L: %f R: %f \r\n", eL, eR);
LtBarbershop 11:521c3e8e6491 361 BtS.printf("Ae L: %f R: %f \n\r", aeL, aeR);
IanTheMBEDMaster 12:3058f9fb09eb 362 BtS.printf("cSP L: %f R: %f \n\r", cSetL, cSetR);
LtBarbershop 13:aaac0105a486 363 BtS.printf("IR F: %f R: %f \n\r", IRF, IRR);
IanTheMBEDMaster 12:3058f9fb09eb 364 Var_Lock.unlock();
LtBarbershop 11:521c3e8e6491 365 Thread::wait(2000);
IanTheMBEDMaster 17:bc13550f673b 366 }
LtBarbershop 11:521c3e8e6491 367
LtBarbershop 11:521c3e8e6491 368 /*
IanTheMBEDMaster 2:1c5cc180812d 369 while(1)
IanTheMBEDMaster 2:1c5cc180812d 370 {
LtBarbershop 10:a3ecedc8d9d7 371 Thread::wait(20);
LtBarbershop 10:a3ecedc8d9d7 372
IanTheMBEDMaster 12:3058f9fb09eb 373
LtBarbershop 10:a3ecedc8d9d7 374
LtBarbershop 4:03bf5bdca9fb 375
IanTheMBEDMaster 12:3058f9fb09eb 376
LtBarbershop 11:521c3e8e6491 377 if(BtS.readable())
LtBarbershop 1:3a40c918ff41 378 {
LtBarbershop 11:521c3e8e6491 379 c = BtS.getc();
LtBarbershop 8:d65cba3bfc0e 380 if (c == 'r')
LtBarbershop 8:d65cba3bfc0e 381 {
LtBarbershop 8:d65cba3bfc0e 382 userSetL = 0.2;
LtBarbershop 8:d65cba3bfc0e 383 }
LtBarbershop 8:d65cba3bfc0e 384 if (c == 'p')
LtBarbershop 8:d65cba3bfc0e 385 {
LtBarbershop 8:d65cba3bfc0e 386 if (RecCount == 100)
LtBarbershop 8:d65cba3bfc0e 387 {
LtBarbershop 11:521c3e8e6491 388 BtS.printf("\n\n\rRecV RecU RecX RecE \n\r");
LtBarbershop 8:d65cba3bfc0e 389 int i = 0;
LtBarbershop 8:d65cba3bfc0e 390 for (i = 0; i < 100; i++)
LtBarbershop 8:d65cba3bfc0e 391 {
LtBarbershop 11:521c3e8e6491 392 BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
LtBarbershop 8:d65cba3bfc0e 393 }
LtBarbershop 8:d65cba3bfc0e 394 }
LtBarbershop 8:d65cba3bfc0e 395
LtBarbershop 8:d65cba3bfc0e 396 }
LtBarbershop 8:d65cba3bfc0e 397
LtBarbershop 8:d65cba3bfc0e 398 if (c == 'n')
LtBarbershop 8:d65cba3bfc0e 399 {
LtBarbershop 11:521c3e8e6491 400 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 11:521c3e8e6491 401 BtS.scanf("%f", &userSetL);
LtBarbershop 8:d65cba3bfc0e 402
LtBarbershop 11:521c3e8e6491 403 BtS.printf("%f", userSetL);
LtBarbershop 8:d65cba3bfc0e 404 userSetR = userSetL;
LtBarbershop 8:d65cba3bfc0e 405 }
IanTheMBEDMaster 2:1c5cc180812d 406 }
IanTheMBEDMaster 2:1c5cc180812d 407
LtBarbershop 10:a3ecedc8d9d7 408 Thread::wait(2000); Wait 2 seconds */
LtBarbershop 1:3a40c918ff41 409 }
LtBarbershop 1:3a40c918ff41 410 }
LtBarbershop 1:3a40c918ff41 411
LtBarbershop 1:3a40c918ff41 412 // ******** Control Thread ********
IanTheMBEDMaster 2:1c5cc180812d 413 void PiControlThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 414 {
LtBarbershop 13:aaac0105a486 415 float maxError = 1.0f;
LtBarbershop 13:aaac0105a486 416 float maxAcc = 10.0f;
LtBarbershop 13:aaac0105a486 417
IanTheMBEDMaster 2:1c5cc180812d 418 while (1)
IanTheMBEDMaster 2:1c5cc180812d 419 {
LtBarbershop 18:4b3ad79d1068 420 // check for emergency stop
LtBarbershop 18:4b3ad79d1068 421 if (EmergencyStop == 1)
LtBarbershop 18:4b3ad79d1068 422 {
LtBarbershop 18:4b3ad79d1068 423 userSetL = 0;
LtBarbershop 18:4b3ad79d1068 424 userSetR = 0;
LtBarbershop 18:4b3ad79d1068 425 SetLeftMotorSpeed(userSetL);
LtBarbershop 18:4b3ad79d1068 426 SetRightMotorSpeed(userSetR);
LtBarbershop 18:4b3ad79d1068 427 BtS.printf("\n\rEmergency Stop!!\n\r");
LtBarbershop 18:4b3ad79d1068 428 }
LtBarbershop 18:4b3ad79d1068 429
IanTheMBEDMaster 2:1c5cc180812d 430 osSignalWait(SignalPi, osWaitForever);
IanTheMBEDMaster 2:1c5cc180812d 431 led2= !led2; // Alive status
IanTheMBEDMaster 2:1c5cc180812d 432
IanTheMBEDMaster 6:5200ce9fa5a7 433 float prevu1, prevu2;
LtBarbershop 13:aaac0105a486 434
LtBarbershop 8:d65cba3bfc0e 435 // Kp = 0.1, Ki = 0.5
LtBarbershop 21:c5713aafdc9d 436 const float Kp = 0.5f;
IanTheMBEDMaster 17:bc13550f673b 437 const float Ki = 0.8f;
LtBarbershop 5:7108ac9e8182 438
LtBarbershop 21:c5713aafdc9d 439 if (action == 2)
LtBarbershop 13:aaac0105a486 440 {
LtBarbershop 13:aaac0105a486 441 IRChecker();
LtBarbershop 13:aaac0105a486 442 }
LtBarbershop 5:7108ac9e8182 443 prevu1 = u1;
LtBarbershop 5:7108ac9e8182 444 prevu2 = u2;
LtBarbershop 4:03bf5bdca9fb 445
IanTheMBEDMaster 6:5200ce9fa5a7 446 // read encoder and calculate speed of both motors
IanTheMBEDMaster 7:751d5e3e9cab 447 GetSpeeds();
LtBarbershop 4:03bf5bdca9fb 448
LtBarbershop 4:03bf5bdca9fb 449 // calculate error
LtBarbershop 5:7108ac9e8182 450 eL = userSetL - fbSpeedL;
LtBarbershop 5:7108ac9e8182 451 eR = userSetR - fbSpeedR;
IanTheMBEDMaster 6:5200ce9fa5a7 452
LtBarbershop 13:aaac0105a486 453 /*
IanTheMBEDMaster 12:3058f9fb09eb 454 // prevent overflow / bound the error
LtBarbershop 13:aaac0105a486 455 if (eL > 1)
LtBarbershop 5:7108ac9e8182 456 {
LtBarbershop 13:aaac0105a486 457 eL = 1;
LtBarbershop 5:7108ac9e8182 458 }
LtBarbershop 13:aaac0105a486 459 if (eL < -1);
LtBarbershop 5:7108ac9e8182 460 {
LtBarbershop 13:aaac0105a486 461 eL = -1;
LtBarbershop 5:7108ac9e8182 462 }
LtBarbershop 5:7108ac9e8182 463 if (eR > maxError)
LtBarbershop 5:7108ac9e8182 464 {
LtBarbershop 5:7108ac9e8182 465 eR = maxError;
LtBarbershop 5:7108ac9e8182 466 }
LtBarbershop 5:7108ac9e8182 467 if (eR < -maxError);
LtBarbershop 5:7108ac9e8182 468 {
LtBarbershop 5:7108ac9e8182 469 eR = -maxError;
IanTheMBEDMaster 12:3058f9fb09eb 470 }
LtBarbershop 13:aaac0105a486 471 */
LtBarbershop 4:03bf5bdca9fb 472 // accumulated error (integration)
LtBarbershop 5:7108ac9e8182 473 if (prevu1 < 1 && prevu1 > -1)
LtBarbershop 5:7108ac9e8182 474 {
LtBarbershop 5:7108ac9e8182 475 aeL += eL;
LtBarbershop 5:7108ac9e8182 476 }
LtBarbershop 5:7108ac9e8182 477 if (prevu2 < 1 && prevu2 > -1)
LtBarbershop 5:7108ac9e8182 478 {
LtBarbershop 5:7108ac9e8182 479 aeR += eR;
LtBarbershop 5:7108ac9e8182 480 }
LtBarbershop 5:7108ac9e8182 481
LtBarbershop 13:aaac0105a486 482 /*
LtBarbershop 5:7108ac9e8182 483 // bound the accumulatd error
LtBarbershop 5:7108ac9e8182 484 if (aeL > maxAcc)
LtBarbershop 5:7108ac9e8182 485 {
LtBarbershop 5:7108ac9e8182 486 aeL = maxAcc;
LtBarbershop 5:7108ac9e8182 487 }
LtBarbershop 5:7108ac9e8182 488 if (aeL < -maxAcc)
LtBarbershop 5:7108ac9e8182 489 {
LtBarbershop 5:7108ac9e8182 490 aeL = -maxAcc;
LtBarbershop 5:7108ac9e8182 491 }
LtBarbershop 5:7108ac9e8182 492 if (aeR > maxAcc)
LtBarbershop 5:7108ac9e8182 493 {
LtBarbershop 5:7108ac9e8182 494 aeR = maxAcc;
LtBarbershop 5:7108ac9e8182 495 }
LtBarbershop 5:7108ac9e8182 496 if (aeR < -maxAcc)
LtBarbershop 5:7108ac9e8182 497 {
LtBarbershop 5:7108ac9e8182 498 aeR = -maxAcc;
LtBarbershop 5:7108ac9e8182 499 }
LtBarbershop 13:aaac0105a486 500 */
LtBarbershop 4:03bf5bdca9fb 501
LtBarbershop 4:03bf5bdca9fb 502 u1 = Kp*eL + Ki*aeL;
LtBarbershop 4:03bf5bdca9fb 503 u2 = Kp*eR + Ki*aeR;
LtBarbershop 5:7108ac9e8182 504
IanTheMBEDMaster 12:3058f9fb09eb 505 // data recording code
IanTheMBEDMaster 17:bc13550f673b 506 if (userSetL == 0.2f || action == 4)
LtBarbershop 8:d65cba3bfc0e 507 {
LtBarbershop 8:d65cba3bfc0e 508 if (RecCount < 100)
LtBarbershop 8:d65cba3bfc0e 509 {
LtBarbershop 21:c5713aafdc9d 510 RecX[RecCount] = aeL;
LtBarbershop 21:c5713aafdc9d 511 RecU[RecCount] = u1;
LtBarbershop 21:c5713aafdc9d 512 RecV[RecCount] = fbSpeedL;
LtBarbershop 21:c5713aafdc9d 513 RecE[RecCount] = eL;
LtBarbershop 8:d65cba3bfc0e 514 RecCount++;
LtBarbershop 8:d65cba3bfc0e 515 }
IanTheMBEDMaster 17:bc13550f673b 516 }
IanTheMBEDMaster 12:3058f9fb09eb 517
IanTheMBEDMaster 17:bc13550f673b 518 SetLeftMotorSpeed(u1);
IanTheMBEDMaster 17:bc13550f673b 519 SetRightMotorSpeed(u2);
IanTheMBEDMaster 2:1c5cc180812d 520 }
IanTheMBEDMaster 2:1c5cc180812d 521 }
LtBarbershop 1:3a40c918ff41 522
LtBarbershop 1:3a40c918ff41 523 // ******** Collision Thread ********
IanTheMBEDMaster 2:1c5cc180812d 524 void ExtCollisionThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 525 {
IanTheMBEDMaster 2:1c5cc180812d 526 while (1)
IanTheMBEDMaster 2:1c5cc180812d 527 {
IanTheMBEDMaster 2:1c5cc180812d 528 osSignalWait(SignalExtCollision, osWaitForever);
LtBarbershop 13:aaac0105a486 529 userSetL = 0;
LtBarbershop 13:aaac0105a486 530 userSetR = 0;
LtBarbershop 13:aaac0105a486 531 SetLeftMotorSpeed(userSetL);
LtBarbershop 13:aaac0105a486 532 SetRightMotorSpeed(userSetR);
LtBarbershop 13:aaac0105a486 533 mbed_reset();
LtBarbershop 1:3a40c918ff41 534 }
LtBarbershop 1:3a40c918ff41 535 }
LtBarbershop 1:3a40c918ff41 536
LtBarbershop 1:3a40c918ff41 537 // ******** Watchdog Interrupt Handler ********
IanTheMBEDMaster 2:1c5cc180812d 538 void Watchdog(void const *n)
IanTheMBEDMaster 2:1c5cc180812d 539 {
LtBarbershop 1:3a40c918ff41 540 led3=1;
LtBarbershop 11:521c3e8e6491 541 BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
LtBarbershop 1:3a40c918ff41 542 }
LtBarbershop 1:3a40c918ff41 543
LtBarbershop 1:3a40c918ff41 544 // ******** Period Timer Interrupt Handler ********
LtBarbershop 5:7108ac9e8182 545 void PiControllerISR(void)
IanTheMBEDMaster 2:1c5cc180812d 546 {
LtBarbershop 1:3a40c918ff41 547 osSignalSet(PiControl,0x1);
IanTheMBEDMaster 2:1c5cc180812d 548 }
LtBarbershop 1:3a40c918ff41 549
LtBarbershop 1:3a40c918ff41 550 // ******** Collision Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 551 void ExtCollisionISR(void)
LtBarbershop 1:3a40c918ff41 552 {
LtBarbershop 1:3a40c918ff41 553 osSignalSet(ExtCollision,0x1);
LtBarbershop 1:3a40c918ff41 554 }
LtBarbershop 1:3a40c918ff41 555
IanTheMBEDMaster 12:3058f9fb09eb 556
IanTheMBEDMaster 2:1c5cc180812d 557 // --- Initialization Functions
IanTheMBEDMaster 2:1c5cc180812d 558 void InitializeSystem()
IanTheMBEDMaster 2:1c5cc180812d 559 {
IanTheMBEDMaster 2:1c5cc180812d 560 led3=0;
IanTheMBEDMaster 2:1c5cc180812d 561 led4=0;
IanTheMBEDMaster 2:1c5cc180812d 562
LtBarbershop 18:4b3ad79d1068 563 //EmerStop.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
IanTheMBEDMaster 2:1c5cc180812d 564
IanTheMBEDMaster 2:1c5cc180812d 565 // Start execution of the Threads
IanTheMBEDMaster 2:1c5cc180812d 566 PiControl = osThreadCreate(osThread(PiControlThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 567 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 568 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
LtBarbershop 4:03bf5bdca9fb 569 PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
IanTheMBEDMaster 2:1c5cc180812d 570
IanTheMBEDMaster 6:5200ce9fa5a7 571 InitializePWM();
IanTheMBEDMaster 2:1c5cc180812d 572 InitializeEncoder();
IanTheMBEDMaster 12:3058f9fb09eb 573 BTInit();
IanTheMBEDMaster 2:1c5cc180812d 574 }
IanTheMBEDMaster 2:1c5cc180812d 575
IanTheMBEDMaster 2:1c5cc180812d 576 void InitializePWM()
IanTheMBEDMaster 2:1c5cc180812d 577 {
IanTheMBEDMaster 6:5200ce9fa5a7 578 PwmL.period(PWMPeriod);
IanTheMBEDMaster 6:5200ce9fa5a7 579 PwmR.period(PWMPeriod);
IanTheMBEDMaster 2:1c5cc180812d 580 }
IanTheMBEDMaster 2:1c5cc180812d 581
IanTheMBEDMaster 2:1c5cc180812d 582 void InitializeEncoder()
IanTheMBEDMaster 2:1c5cc180812d 583 {
IanTheMBEDMaster 2:1c5cc180812d 584 // Initialization – to be executed once (normally)
IanTheMBEDMaster 2:1c5cc180812d 585 DE0.format(16,0); // SPI format: 16-bit words, mode 0 protocol.
LtBarbershop 4:03bf5bdca9fb 586 DE0.frequency(1000000);
IanTheMBEDMaster 2:1c5cc180812d 587 SpiStart = 0;
IanTheMBEDMaster 2:1c5cc180812d 588 SpiReset = 1;
IanTheMBEDMaster 2:1c5cc180812d 589 wait_us(10);
IanTheMBEDMaster 2:1c5cc180812d 590 SpiReset = 0;
IanTheMBEDMaster 2:1c5cc180812d 591 DE0.write(0x8004); // SPI slave control word to read (only) 4-word transactions
IanTheMBEDMaster 2:1c5cc180812d 592 // starting at base address 0 within the peripheral.
IanTheMBEDMaster 2:1c5cc180812d 593 }
IanTheMBEDMaster 2:1c5cc180812d 594
IanTheMBEDMaster 12:3058f9fb09eb 595 void BTInit()
IanTheMBEDMaster 12:3058f9fb09eb 596 {
IanTheMBEDMaster 12:3058f9fb09eb 597 BtS.printf("AT+BTCANCEL\r\n");
IanTheMBEDMaster 12:3058f9fb09eb 598 BtS.printf("AT+BTSCAN\r\n");
IanTheMBEDMaster 14:669f2f1566b0 599
IanTheMBEDMaster 14:669f2f1566b0 600 // also send to the pc so we know whats going on
IanTheMBEDMaster 14:669f2f1566b0 601 pc.printf("AT+BTCANCEL\r\n");
IanTheMBEDMaster 14:669f2f1566b0 602 pc.printf("AT+BTSCAN\r\n");
IanTheMBEDMaster 12:3058f9fb09eb 603 }
IanTheMBEDMaster 2:1c5cc180812d 604
IanTheMBEDMaster 2:1c5cc180812d 605 // --- Other Functions
IanTheMBEDMaster 2:1c5cc180812d 606 void SetLeftMotorSpeed(float u)
LtBarbershop 1:3a40c918ff41 607 {
IanTheMBEDMaster 2:1c5cc180812d 608 float T;
IanTheMBEDMaster 2:1c5cc180812d 609 float d;
IanTheMBEDMaster 2:1c5cc180812d 610 float onTime;
IanTheMBEDMaster 20:344c088d3ead 611 float maxMotorSpeed = 0.5;
IanTheMBEDMaster 2:1c5cc180812d 612
LtBarbershop 3:9a39e487b724 613 // bound the input
IanTheMBEDMaster 20:344c088d3ead 614 if (u > maxMotorSpeed)
LtBarbershop 3:9a39e487b724 615 {
IanTheMBEDMaster 20:344c088d3ead 616 u = maxMotorSpeed;
LtBarbershop 3:9a39e487b724 617 }
LtBarbershop 3:9a39e487b724 618
IanTheMBEDMaster 20:344c088d3ead 619 if (u < -maxMotorSpeed)
LtBarbershop 3:9a39e487b724 620 {
IanTheMBEDMaster 20:344c088d3ead 621 u = -maxMotorSpeed;
LtBarbershop 3:9a39e487b724 622 }
LtBarbershop 3:9a39e487b724 623
LtBarbershop 3:9a39e487b724 624 // calculate duty cycle timing
IanTheMBEDMaster 2:1c5cc180812d 625 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 626 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 627 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 628
IanTheMBEDMaster 2:1c5cc180812d 629 PwmL.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 630
IanTheMBEDMaster 2:1c5cc180812d 631 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 632 {
IanTheMBEDMaster 2:1c5cc180812d 633 dirL = 1;
IanTheMBEDMaster 2:1c5cc180812d 634 }
IanTheMBEDMaster 2:1c5cc180812d 635 else
IanTheMBEDMaster 2:1c5cc180812d 636 {
IanTheMBEDMaster 2:1c5cc180812d 637 dirL = 0;
IanTheMBEDMaster 2:1c5cc180812d 638 }
IanTheMBEDMaster 2:1c5cc180812d 639 }
LtBarbershop 1:3a40c918ff41 640
IanTheMBEDMaster 2:1c5cc180812d 641 void SetRightMotorSpeed(float u)
IanTheMBEDMaster 2:1c5cc180812d 642 {
IanTheMBEDMaster 2:1c5cc180812d 643 float T;
IanTheMBEDMaster 2:1c5cc180812d 644 float d;
IanTheMBEDMaster 2:1c5cc180812d 645 float onTime;
IanTheMBEDMaster 20:344c088d3ead 646 float maxMotorSpeed = 0.5;
IanTheMBEDMaster 2:1c5cc180812d 647
LtBarbershop 3:9a39e487b724 648 // bound the input
IanTheMBEDMaster 20:344c088d3ead 649 if (u > maxMotorSpeed)
LtBarbershop 3:9a39e487b724 650 {
IanTheMBEDMaster 20:344c088d3ead 651 u = maxMotorSpeed;
LtBarbershop 3:9a39e487b724 652 }
LtBarbershop 3:9a39e487b724 653
IanTheMBEDMaster 20:344c088d3ead 654 if (u < -maxMotorSpeed)
LtBarbershop 3:9a39e487b724 655 {
IanTheMBEDMaster 20:344c088d3ead 656 u = -maxMotorSpeed;
LtBarbershop 3:9a39e487b724 657 }
LtBarbershop 3:9a39e487b724 658
LtBarbershop 3:9a39e487b724 659 // calculate duty cycle timing
IanTheMBEDMaster 2:1c5cc180812d 660 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 661 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 662 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 663
IanTheMBEDMaster 2:1c5cc180812d 664 PwmR.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 665
IanTheMBEDMaster 2:1c5cc180812d 666 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 667 {
IanTheMBEDMaster 2:1c5cc180812d 668 dirR = 1;
IanTheMBEDMaster 2:1c5cc180812d 669 }
IanTheMBEDMaster 2:1c5cc180812d 670 else
IanTheMBEDMaster 2:1c5cc180812d 671 {
IanTheMBEDMaster 2:1c5cc180812d 672 dirR = 0;
IanTheMBEDMaster 2:1c5cc180812d 673 }
LtBarbershop 1:3a40c918ff41 674 }
LtBarbershop 1:3a40c918ff41 675
IanTheMBEDMaster 7:751d5e3e9cab 676 void GetSpeeds()
LtBarbershop 1:3a40c918ff41 677 {
IanTheMBEDMaster 22:3cda0d788452 678 // when using a interrupt period of 50ms: 1480
IanTheMBEDMaster 22:3cda0d788452 679 float leftMaxPos = 1184.0f;
IanTheMBEDMaster 22:3cda0d788452 680 float rightMaxPos = 1184.0f;
LtBarbershop 1:3a40c918ff41 681
IanTheMBEDMaster 6:5200ce9fa5a7 682 // Restart the SPI module each time
LtBarbershop 4:03bf5bdca9fb 683 SpiStart = 1;
LtBarbershop 4:03bf5bdca9fb 684 wait_us(5);
LtBarbershop 4:03bf5bdca9fb 685 SpiStart = 0;
LtBarbershop 4:03bf5bdca9fb 686 DE0.write(0x8004);
IanTheMBEDMaster 12:3058f9fb09eb 687
IanTheMBEDMaster 6:5200ce9fa5a7 688 // read in 4 16-bit words
LtBarbershop 4:03bf5bdca9fb 689 Var_Lock.lock();
IanTheMBEDMaster 6:5200ce9fa5a7 690 dPositionLeft = DE0.write(Dummy); // Read QEI-0 position register
IanTheMBEDMaster 6:5200ce9fa5a7 691 dTimeLeft = DE0.write(Dummy); // Read QE-0 time interval register
LtBarbershop 4:03bf5bdca9fb 692 dPositionRight = DE0.write(Dummy); // Read QEI-1 position register
LtBarbershop 4:03bf5bdca9fb 693 dTimeRight = DE0.write(Dummy); // Read QEI-1 time interval register
IanTheMBEDMaster 6:5200ce9fa5a7 694 Var_Lock.unlock();
LtBarbershop 4:03bf5bdca9fb 695
IanTheMBEDMaster 7:751d5e3e9cab 696 // calcspeed
IanTheMBEDMaster 7:751d5e3e9cab 697 fbSpeedL = ((float)dPositionLeft)/leftMaxPos;
IanTheMBEDMaster 7:751d5e3e9cab 698 fbSpeedR = ((float)dPositionRight)/rightMaxPos;
IanTheMBEDMaster 7:751d5e3e9cab 699
IanTheMBEDMaster 6:5200ce9fa5a7 700 // bound the feedback speed
IanTheMBEDMaster 7:751d5e3e9cab 701 if (fbSpeedL > 1)
IanTheMBEDMaster 6:5200ce9fa5a7 702 {
IanTheMBEDMaster 7:751d5e3e9cab 703 fbSpeedL = 1;
IanTheMBEDMaster 6:5200ce9fa5a7 704 }
IanTheMBEDMaster 7:751d5e3e9cab 705 if (fbSpeedL < -1)
IanTheMBEDMaster 6:5200ce9fa5a7 706 {
IanTheMBEDMaster 7:751d5e3e9cab 707 fbSpeedL = -1;
IanTheMBEDMaster 6:5200ce9fa5a7 708 }
IanTheMBEDMaster 7:751d5e3e9cab 709 if (fbSpeedR > 1)
IanTheMBEDMaster 6:5200ce9fa5a7 710 {
IanTheMBEDMaster 7:751d5e3e9cab 711 fbSpeedR = 1;
IanTheMBEDMaster 6:5200ce9fa5a7 712 }
IanTheMBEDMaster 7:751d5e3e9cab 713 if (fbSpeedR < -1)
IanTheMBEDMaster 6:5200ce9fa5a7 714 {
IanTheMBEDMaster 7:751d5e3e9cab 715 fbSpeedR = -1;
IanTheMBEDMaster 6:5200ce9fa5a7 716 }
LtBarbershop 9:0eb7b173d6c3 717 }
LtBarbershop 11:521c3e8e6491 718
LtBarbershop 11:521c3e8e6491 719 void IRChecker()
LtBarbershop 9:0eb7b173d6c3 720 {
IanTheMBEDMaster 17:bc13550f673b 721 float IRF, IRR, IRF1, IRR1, IRF2, IRR2, prevDF, prevDR;
LtBarbershop 13:aaac0105a486 722 float aF = 22.6321; // 34.2911
LtBarbershop 13:aaac0105a486 723 float bF = -0.1721; // -0.2608
LtBarbershop 13:aaac0105a486 724 float aR = 22.6021; // 34.2456
LtBarbershop 13:aaac0105a486 725 float bR = -0.0376; // -0.0569
IanTheMBEDMaster 14:669f2f1566b0 726
IanTheMBEDMaster 14:669f2f1566b0 727 // Read Sensors
IanTheMBEDMaster 17:bc13550f673b 728 IRF1 = 3.3*IRFront.read();
IanTheMBEDMaster 17:bc13550f673b 729 IRR1 = 3.3*IRRear.read();
IanTheMBEDMaster 17:bc13550f673b 730 IRF2 = 3.3*IRFront.read();
IanTheMBEDMaster 17:bc13550f673b 731 IRR2 = 3.3*IRRear.read();
IanTheMBEDMaster 17:bc13550f673b 732
IanTheMBEDMaster 17:bc13550f673b 733 // average
IanTheMBEDMaster 17:bc13550f673b 734 IRF = (IRF1 + IRF2)/2;
IanTheMBEDMaster 17:bc13550f673b 735 IRR = (IRR1 + IRR2)/2;
IanTheMBEDMaster 17:bc13550f673b 736
LtBarbershop 16:cf9519c35510 737 prevDF = DF;
LtBarbershop 16:cf9519c35510 738 prevDR = DR;
LtBarbershop 18:4b3ad79d1068 739
IanTheMBEDMaster 19:b9c4952f4acc 740 // Calculate distance based on voltage
IanTheMBEDMaster 14:669f2f1566b0 741 DF = aF/(IRF+bF);
IanTheMBEDMaster 14:669f2f1566b0 742 DR = aR/(IRR+bR);
IanTheMBEDMaster 14:669f2f1566b0 743
IanTheMBEDMaster 17:bc13550f673b 744 prevuW = uW;
IanTheMBEDMaster 17:bc13550f673b 745
LtBarbershop 16:cf9519c35510 746 // check for invalid data
IanTheMBEDMaster 19:b9c4952f4acc 747 if (DF < 0)
IanTheMBEDMaster 19:b9c4952f4acc 748 {
IanTheMBEDMaster 19:b9c4952f4acc 749 DF = 80;
IanTheMBEDMaster 19:b9c4952f4acc 750 }
IanTheMBEDMaster 19:b9c4952f4acc 751 if (DR < 0)
IanTheMBEDMaster 19:b9c4952f4acc 752 {
IanTheMBEDMaster 19:b9c4952f4acc 753 DR = 80;
IanTheMBEDMaster 19:b9c4952f4acc 754 }
LtBarbershop 21:c5713aafdc9d 755 if (DF > 80)
LtBarbershop 21:c5713aafdc9d 756 {
LtBarbershop 21:c5713aafdc9d 757 DF = 80;
LtBarbershop 21:c5713aafdc9d 758 }
LtBarbershop 21:c5713aafdc9d 759 if (DR > 80)
LtBarbershop 21:c5713aafdc9d 760 {
LtBarbershop 21:c5713aafdc9d 761 DR = 80;
LtBarbershop 21:c5713aafdc9d 762 }
LtBarbershop 21:c5713aafdc9d 763 if (DF < 10)
LtBarbershop 21:c5713aafdc9d 764 {
LtBarbershop 21:c5713aafdc9d 765 DF = 10;
LtBarbershop 21:c5713aafdc9d 766 }
LtBarbershop 21:c5713aafdc9d 767 if (DR < 10)
LtBarbershop 21:c5713aafdc9d 768 {
LtBarbershop 21:c5713aafdc9d 769 DR = 10;
LtBarbershop 21:c5713aafdc9d 770 }
LtBarbershop 21:c5713aafdc9d 771
IanTheMBEDMaster 14:669f2f1566b0 772 // calculate errors
IanTheMBEDMaster 14:669f2f1566b0 773 eW = Kpos*(Nominal - (DF + DR)/2) + Kor*(DR - DF);
IanTheMBEDMaster 14:669f2f1566b0 774
IanTheMBEDMaster 14:669f2f1566b0 775 // accumulate error
LtBarbershop 18:4b3ad79d1068 776 if (prevuW < Turn && prevuW > -Turn)
IanTheMBEDMaster 17:bc13550f673b 777 {
IanTheMBEDMaster 17:bc13550f673b 778 aeW = aeW + eW;
IanTheMBEDMaster 17:bc13550f673b 779 }
IanTheMBEDMaster 14:669f2f1566b0 780
IanTheMBEDMaster 14:669f2f1566b0 781 uW = KpW*eW + KiW*aeW;
IanTheMBEDMaster 14:669f2f1566b0 782
LtBarbershop 18:4b3ad79d1068 783 if (uW > Turn)
IanTheMBEDMaster 15:407e4152cf3a 784 {
LtBarbershop 18:4b3ad79d1068 785 uW = Turn;
IanTheMBEDMaster 15:407e4152cf3a 786 }
LtBarbershop 18:4b3ad79d1068 787 else if (uW < -Turn)
IanTheMBEDMaster 15:407e4152cf3a 788 {
LtBarbershop 18:4b3ad79d1068 789 uW = -Turn;
IanTheMBEDMaster 15:407e4152cf3a 790 }
IanTheMBEDMaster 15:407e4152cf3a 791
IanTheMBEDMaster 14:669f2f1566b0 792 // set differential speeds
IanTheMBEDMaster 14:669f2f1566b0 793 userSetL = TSpeedL + uW;
IanTheMBEDMaster 14:669f2f1566b0 794 userSetR = TSpeedR - uW;
IanTheMBEDMaster 14:669f2f1566b0 795 }
IanTheMBEDMaster 14:669f2f1566b0 796
IanTheMBEDMaster 14:669f2f1566b0 797 /*
IanTheMBEDMaster 14:669f2f1566b0 798 void IRChecker()
IanTheMBEDMaster 14:669f2f1566b0 799 {
IanTheMBEDMaster 14:669f2f1566b0 800 float IRF, IRR;
IanTheMBEDMaster 14:669f2f1566b0 801 float aF = 22.6321; // 34.2911
IanTheMBEDMaster 14:669f2f1566b0 802 float bF = -0.1721; // -0.2608
IanTheMBEDMaster 14:669f2f1566b0 803 float aR = 22.6021; // 34.2456
IanTheMBEDMaster 14:669f2f1566b0 804 float bR = -0.0376; // -0.0569
LtBarbershop 18:4b3ad79d1068 805 // float Nominal = 25;
LtBarbershop 13:aaac0105a486 806
LtBarbershop 9:0eb7b173d6c3 807 // Read Sensors
LtBarbershop 13:aaac0105a486 808 IRF = 3.3*IRFront.read();
LtBarbershop 13:aaac0105a486 809 IRR = 3.3*IRRear.read();
LtBarbershop 13:aaac0105a486 810
LtBarbershop 9:0eb7b173d6c3 811
LtBarbershop 13:aaac0105a486 812 // Calculate distance based on voltage
LtBarbershop 13:aaac0105a486 813 DF = aF/(IRF+bF);
LtBarbershop 13:aaac0105a486 814 DR = aR/(IRR+bR);
LtBarbershop 9:0eb7b173d6c3 815
LtBarbershop 13:aaac0105a486 816 // Display Distances
LtBarbershop 13:aaac0105a486 817 // BtS.printf("IRD F: %f R: %f\n\r\n", DF, DR);
LtBarbershop 13:aaac0105a486 818
LtBarbershop 13:aaac0105a486 819 // Compare to nominal voltage and adjust
LtBarbershop 13:aaac0105a486 820 while(1)
LtBarbershop 9:0eb7b173d6c3 821 {
LtBarbershop 13:aaac0105a486 822 if (DF > Nominal && DR > Nominal)
LtBarbershop 13:aaac0105a486 823 {
LtBarbershop 13:aaac0105a486 824 userSetR = TSpeedR + Turn;
LtBarbershop 13:aaac0105a486 825 userSetL = TSpeedL - Turn;
LtBarbershop 13:aaac0105a486 826 break;
LtBarbershop 13:aaac0105a486 827 }
LtBarbershop 13:aaac0105a486 828 if (DF < Nominal && DR < Nominal)
LtBarbershop 13:aaac0105a486 829 {
LtBarbershop 13:aaac0105a486 830 userSetR = TSpeedR - Turn;
LtBarbershop 13:aaac0105a486 831 userSetL = TSpeedL + Turn;
LtBarbershop 13:aaac0105a486 832 break;
LtBarbershop 13:aaac0105a486 833 }
LtBarbershop 13:aaac0105a486 834
LtBarbershop 13:aaac0105a486 835 // Compare rangers to each other
LtBarbershop 13:aaac0105a486 836 if (DF > DR) // IRF further than IRR
LtBarbershop 13:aaac0105a486 837 {
LtBarbershop 13:aaac0105a486 838 userSetR = TSpeedR + Turn;
LtBarbershop 13:aaac0105a486 839 userSetL = TSpeedL - Turn;
LtBarbershop 13:aaac0105a486 840 break;
LtBarbershop 13:aaac0105a486 841 }
LtBarbershop 13:aaac0105a486 842 if (DF < DR) // IRF closer than IRR
LtBarbershop 13:aaac0105a486 843 {
LtBarbershop 13:aaac0105a486 844 userSetR = TSpeedR - Turn;
LtBarbershop 13:aaac0105a486 845 userSetL = TSpeedL + Turn;
LtBarbershop 13:aaac0105a486 846 break;
LtBarbershop 13:aaac0105a486 847 }
LtBarbershop 13:aaac0105a486 848 }
LtBarbershop 11:521c3e8e6491 849 }
IanTheMBEDMaster 14:669f2f1566b0 850 */
LtBarbershop 10:a3ecedc8d9d7 851
LtBarbershop 11:521c3e8e6491 852 void DisplayMenu()
LtBarbershop 11:521c3e8e6491 853 {
IanTheMBEDMaster 12:3058f9fb09eb 854 BtS.printf("\r\n\nPress the corresponding key to do something:\r\n");
LtBarbershop 11:521c3e8e6491 855 BtS.printf("| Key | Action\n\r");
IanTheMBEDMaster 12:3058f9fb09eb 856 BtS.printf("|-----|----------------------------\n\r");
LtBarbershop 11:521c3e8e6491 857 BtS.printf("| d | Drive the robot using wasd keys\n\r");
LtBarbershop 11:521c3e8e6491 858 BtS.printf("| w | Robot performs wall following\n\r");
LtBarbershop 11:521c3e8e6491 859 BtS.printf("| 0 | Debug mode\n\r");
IanTheMBEDMaster 17:bc13550f673b 860 BtS.printf("| r | Record Data \n\r");
IanTheMBEDMaster 12:3058f9fb09eb 861 BtS.printf("| q | Quit current action, stop the robot, and return to this menu\n\r\n");
LtBarbershop 11:521c3e8e6491 862 }
LtBarbershop 11:521c3e8e6491 863
LtBarbershop 11:521c3e8e6491 864 void Ramp(float speed, unsigned int time, unsigned short motor)
LtBarbershop 11:521c3e8e6491 865 {
LtBarbershop 11:521c3e8e6491 866 const unsigned short steps = 20;
LtBarbershop 11:521c3e8e6491 867 float changeL = (speed - userSetL)/steps;
LtBarbershop 11:521c3e8e6491 868 float changeR = (speed - userSetR)/steps;
LtBarbershop 11:521c3e8e6491 869 unsigned short i;
LtBarbershop 11:521c3e8e6491 870 // calculate wait time (we wont worry too much about rounding errors)
LtBarbershop 11:521c3e8e6491 871 unsigned int waitTime = time/steps;
LtBarbershop 11:521c3e8e6491 872
LtBarbershop 11:521c3e8e6491 873 for (i = 0; i < steps; i++)
LtBarbershop 11:521c3e8e6491 874 {
LtBarbershop 11:521c3e8e6491 875 //Thread::wait(200);
LtBarbershop 11:521c3e8e6491 876 Thread::wait(waitTime);
LtBarbershop 11:521c3e8e6491 877
LtBarbershop 11:521c3e8e6491 878 if (motor == 0)
LtBarbershop 11:521c3e8e6491 879 {
LtBarbershop 11:521c3e8e6491 880 // change both speeds
LtBarbershop 11:521c3e8e6491 881 userSetL += changeL;
LtBarbershop 11:521c3e8e6491 882 userSetR += changeR;
LtBarbershop 11:521c3e8e6491 883 continue;
LtBarbershop 11:521c3e8e6491 884 }
LtBarbershop 11:521c3e8e6491 885 if (motor == 1)
LtBarbershop 11:521c3e8e6491 886 {
LtBarbershop 11:521c3e8e6491 887 userSetL += changeL;
LtBarbershop 11:521c3e8e6491 888 continue;
LtBarbershop 11:521c3e8e6491 889 }
LtBarbershop 11:521c3e8e6491 890 if (motor == 2)
LtBarbershop 11:521c3e8e6491 891 {
LtBarbershop 11:521c3e8e6491 892 userSetR += changeR;
IanTheMBEDMaster 12:3058f9fb09eb 893 }
IanTheMBEDMaster 12:3058f9fb09eb 894 }
LtBarbershop 9:0eb7b173d6c3 895 }