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@20:344c088d3ead, 2013-04-05 (annotated)

Committer:
IanTheMBEDMaster
Date:
Fri Apr 05 17:41:23 2013 +0000
Revision:
20:344c088d3ead
Parent:
19:b9c4952f4acc
Child:
21:c5713aafdc9d
a few more changes

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