Will Zhao / Mbed 2 deprecated Subsystem

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

main.cpp@1:f1d9b55f7dd7, 2016-03-05 (annotated)

Committer:
ibestwill
Date:
Sat Mar 05 02:44:45 2016 +0000
Revision:
1:f1d9b55f7dd7
Parent:
0:867c4eabf128 
was

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ibestwill 0:867c4eabf128 1 #include "mbed.h"
ibestwill 0:867c4eabf128 2 #include "rtos.h"
ibestwill 0:867c4eabf128 3 #include "stdlib.h"
ibestwill 0:867c4eabf128 4
ibestwill 0:867c4eabf128 5 // IO Port Configuration
ibestwill 0:867c4eabf128 6 DigitalOut led1(LED1);
ibestwill 0:867c4eabf128 7 DigitalOut led2(LED2);
ibestwill 0:867c4eabf128 8 DigitalOut led3(LED3);
ibestwill 0:867c4eabf128 9 DigitalOut led4(LED4);
ibestwill 0:867c4eabf128 10 PwmOut PwmR(p21); // define p21 as PwmR, p22 as PwmL
ibestwill 0:867c4eabf128 11 PwmOut PwmL(p22); // define p21 as PwmR, p22 as PwmL
ibestwill 0:867c4eabf128 12 DigitalOut DirR(p30); // define p30 as DirR, p29 as DirL
ibestwill 0:867c4eabf128 13 DigitalOut DirL(p29); // define p30 as DirR, p29 as DirL
ibestwill 0:867c4eabf128 14 DigitalOut SpiReset(p11);//Reset for all devices within the slave SPI peripheral in the DE0 FPGA
ibestwill 0:867c4eabf128 15 DigitalOut IoReset(p12); //Places SPI interface on the DE0 FPGA into control mode
ibestwill 0:867c4eabf128 16 SPI DE0(p5,p6,p7); //(mosi,miso,sclk)DE0 is the SPI channel with the FPGA
ibestwill 0:867c4eabf128 17 Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
ibestwill 0:867c4eabf128 18 Serial BluetoothSerial(p9, p10); // tx, rx
ibestwill 0:867c4eabf128 19 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
ibestwill 0:867c4eabf128 20 InterruptIn Bumper(p8); // External interrupt pin declared as Bumper
ibestwill 0:867c4eabf128 21
ibestwill 0:867c4eabf128 22 // Function prototypes
ibestwill 0:867c4eabf128 23 void Initialize(void);
ibestwill 0:867c4eabf128 24 char DisplayMenu(void);
ibestwill 0:867c4eabf128 25 void ManualControl(void);
ibestwill 1:f1d9b55f7dd7 26 void BluetoothModule(void);
ibestwill 0:867c4eabf128 27 void PiControllerISR(void);
ibestwill 0:867c4eabf128 28 void WdtFaultISR(void);
ibestwill 0:867c4eabf128 29 void ExtCollisionISR(void);
ibestwill 0:867c4eabf128 30 void PiControlThread(void const *argument);
ibestwill 0:867c4eabf128 31 void ExtCollisionThread(void const *argument);
ibestwill 0:867c4eabf128 32 void Watchdog(void const *n);
ibestwill 0:867c4eabf128 33 void PIController(void);
ibestwill 0:867c4eabf128 34 void SpeedControl(void);
ibestwill 0:867c4eabf128 35 int SatAdd(int x, int y);
ibestwill 0:867c4eabf128 36 int SatSub(int x, int y);
ibestwill 0:867c4eabf128 37 int SatValue(int x, int Limit);
ibestwill 0:867c4eabf128 38 int Signextend(int x);
ibestwill 0:867c4eabf128 39 void Userinterface(void);
ibestwill 0:867c4eabf128 40 void UsRangefinder(void);
ibestwill 0:867c4eabf128 41 void RcServoMotor(void);
ibestwill 0:867c4eabf128 42
ibestwill 0:867c4eabf128 43
ibestwill 0:867c4eabf128 44 // Processes and threads
ibestwill 0:867c4eabf128 45 int32_t SignalPi, SignalWdt, SignalExtCollision;
ibestwill 0:867c4eabf128 46 osThreadId PiControl,WdtFault,ExtCollision;
ibestwill 0:867c4eabf128 47 osThreadDef(PiControlThread, osPriorityRealtime, DEFAULT_STACK_SIZE); // Declare Control as a thread/process
ibestwill 0:867c4eabf128 48 osThreadDef(ExtCollisionThread, osPriorityHigh, DEFAULT_STACK_SIZE); // Declare External Collision as a thread/process
ibestwill 0:867c4eabf128 49 osTimerDef(Wdtimer, Watchdog); // Declare a watch dog timer
ibestwill 0:867c4eabf128 50
ibestwill 0:867c4eabf128 51
ibestwill 0:867c4eabf128 52 // Global variables for interrupt handler
ibestwill 1:f1d9b55f7dd7 53 // Global variables for interrupt handler
ibestwill 0:867c4eabf128 54 int32_t dPos_R, dTime_R, Vel_R;
ibestwill 1:f1d9b55f7dd7 55 int32_t dPos_L, dTime_L, Vel_L;
ibestwill 1:f1d9b55f7dd7 56 int32_t Setspeed_R=0;
ibestwill 1:f1d9b55f7dd7 57 int32_t Setspeed_L=0;
ibestwill 0:867c4eabf128 58 int32_t u_R=0,uS_R=0;
ibestwill 1:f1d9b55f7dd7 59 int32_t u_L=0,uS_L=0;
ibestwill 0:867c4eabf128 60 int32_t uP_R,uI_R;
ibestwill 1:f1d9b55f7dd7 61 int32_t uP_L,uI_L;
ibestwill 1:f1d9b55f7dd7 62 int32_t e_R,e_L;
ibestwill 0:867c4eabf128 63 int32_t x_R=0, xT_R=0;
ibestwill 1:f1d9b55f7dd7 64 int32_t x_L=0, xT_L=0;
ibestwill 0:867c4eabf128 65
ibestwill 0:867c4eabf128 66 float T = 5000; //us
ibestwill 0:867c4eabf128 67 float y_R=0;
ibestwill 1:f1d9b55f7dd7 68 float y_L=0;
ibestwill 1:f1d9b55f7dd7 69 float Kp_R=2.5, Ki_R=0.01;
ibestwill 1:f1d9b55f7dd7 70 float Kp_L=2.5, Ki_L=0.01;
ibestwill 1:f1d9b55f7dd7 71
ibestwill 0:867c4eabf128 72
ibestwill 0:867c4eabf128 73
ibestwill 0:867c4eabf128 74 //**************************************************************************
ibestwill 0:867c4eabf128 75 //**************************************************************************
ibestwill 0:867c4eabf128 76
ibestwill 0:867c4eabf128 77
ibestwill 0:867c4eabf128 78 int main()
ibestwill 0:867c4eabf128 79 {
ibestwill 0:867c4eabf128 80
ibestwill 1:f1d9b55f7dd7 81
ibestwill 1:f1d9b55f7dd7 82 Initialize();
ibestwill 0:867c4eabf128 83
ibestwill 0:867c4eabf128 84 //Userinterface();
ibestwill 0:867c4eabf128 85
ibestwill 0:867c4eabf128 86 //UsRangefinder();
ibestwill 0:867c4eabf128 87
ibestwill 1:f1d9b55f7dd7 88 //RcServoMotor();
ibestwill 1:f1d9b55f7dd7 89
ibestwill 1:f1d9b55f7dd7 90
ibestwill 1:f1d9b55f7dd7 91
ibestwill 1:f1d9b55f7dd7 92 PeriodicInt.attach(&PiControllerISR, 0.0005);
ibestwill 1:f1d9b55f7dd7 93
ibestwill 1:f1d9b55f7dd7 94
ibestwill 1:f1d9b55f7dd7 95 do {
ibestwill 1:f1d9b55f7dd7 96
ibestwill 1:f1d9b55f7dd7 97 //BluetoothModule();
ibestwill 1:f1d9b55f7dd7 98
ibestwill 1:f1d9b55f7dd7 99 Userinterface();
ibestwill 1:f1d9b55f7dd7 100
ibestwill 1:f1d9b55f7dd7 101 pc.printf("\r\nSR:%d Vel_R:%d ER:%d XR:%d uR:%d \r\n" ,Setspeed_R,Vel_R,e_R,x_R,u_R);
ibestwill 1:f1d9b55f7dd7 102
ibestwill 1:f1d9b55f7dd7 103 pc.printf("\r\nSL:%d Vel_L:%d EL:%d XL:%d uL:%d \r\n\n\n" ,Setspeed_R,Vel_R,e_R,x_R,u_R);
ibestwill 1:f1d9b55f7dd7 104
ibestwill 1:f1d9b55f7dd7 105
ibestwill 1:f1d9b55f7dd7 106 wait(0.5); // Go to sleep for 500 ms
ibestwill 1:f1d9b55f7dd7 107
ibestwill 1:f1d9b55f7dd7 108 } while(true);
ibestwill 1:f1d9b55f7dd7 109
ibestwill 1:f1d9b55f7dd7 110
ibestwill 0:867c4eabf128 111
ibestwill 0:867c4eabf128 112 }
ibestwill 0:867c4eabf128 113
ibestwill 0:867c4eabf128 114
ibestwill 1:f1d9b55f7dd7 115
ibestwill 1:f1d9b55f7dd7 116
ibestwill 1:f1d9b55f7dd7 117 //------------------------------------------------------------
ibestwill 1:f1d9b55f7dd7 118 void BluetoothModule(void) {
ibestwill 1:f1d9b55f7dd7 119 // Echo characters at serial Bluetooth Channel (p9, p10)to and from the pc.serial.port
ibestwill 1:f1d9b55f7dd7 120 // Connect TxD from Bluetooth to rx (p10); RxD from Bluetooth to tx (p9)
ibestwill 1:f1d9b55f7dd7 121 char x;
ibestwill 1:f1d9b55f7dd7 122 BluetoothSerial.baud(9600);
ibestwill 1:f1d9b55f7dd7 123 printf("\r\nBluetooth serial channel.\r\n");
ibestwill 1:f1d9b55f7dd7 124 x=0;
ibestwill 1:f1d9b55f7dd7 125
ibestwill 1:f1d9b55f7dd7 126 if (pc.readable()){
ibestwill 1:f1d9b55f7dd7 127 x=pc.getc();
ibestwill 1:f1d9b55f7dd7 128 BluetoothSerial.putc(x); // Receive keyboard entry and send to bluetooth
ibestwill 1:f1d9b55f7dd7 129 pc.putc(x); //Echo character from MBED COMM
ibestwill 1:f1d9b55f7dd7 130 }
ibestwill 1:f1d9b55f7dd7 131 if (BluetoothSerial.readable()){
ibestwill 1:f1d9b55f7dd7 132 x=BluetoothSerial.getc(); // Receive character from bluetooth module
ibestwill 1:f1d9b55f7dd7 133 pc.putc(x); // Write character from Bluetooth module to terminal emulator on PC
ibestwill 1:f1d9b55f7dd7 134 BluetoothSerial.putc(x); // Echo character character from bluetooth COMM
ibestwill 1:f1d9b55f7dd7 135 }
ibestwill 1:f1d9b55f7dd7 136
ibestwill 1:f1d9b55f7dd7 137 }
ibestwill 1:f1d9b55f7dd7 138
ibestwill 1:f1d9b55f7dd7 139
ibestwill 0:867c4eabf128 140 void UsRangefinder(void)
ibestwill 0:867c4eabf128 141 {
ibestwill 0:867c4eabf128 142 unsigned int V, duration;
ibestwill 0:867c4eabf128 143 float distance;
ibestwill 0:867c4eabf128 144 DE0.format(16,1); // SPI is mode 1
ibestwill 0:867c4eabf128 145 DE0.frequency(500000); // SPI frequency
ibestwill 0:867c4eabf128 146
ibestwill 0:867c4eabf128 147 IoReset=0; // Reset all SPI peripherals
ibestwill 0:867c4eabf128 148 IoReset=1;
ibestwill 0:867c4eabf128 149 wait_us(5);
ibestwill 0:867c4eabf128 150 IoReset=0;
ibestwill 0:867c4eabf128 151
ibestwill 0:867c4eabf128 152 SpiReset=0;
ibestwill 0:867c4eabf128 153 SpiReset=1;
ibestwill 0:867c4eabf128 154 wait_us(5);
ibestwill 0:867c4eabf128 155 SpiReset=0;
ibestwill 0:867c4eabf128 156 V=DE0.write(0x8901); // Control word to read ultrasonic range finder.
ibestwill 0:867c4eabf128 157 do {
ibestwill 0:867c4eabf128 158 duration = DE0.write(0x0000); // Read ultrasonic rangefinder.
ibestwill 0:867c4eabf128 159 pc.printf("\r\n SPI ID=%4x Us Range=%5d",V,duration);
ibestwill 0:867c4eabf128 160
ibestwill 0:867c4eabf128 161 //Need to check if duration is singed or unsigned value, if extend 16bit to 32bit
ibestwill 0:867c4eabf128 162 distance = (duration / 2) * 0.0344;
ibestwill 0:867c4eabf128 163
ibestwill 0:867c4eabf128 164 if (distance >= 400 || distance <= 2){
ibestwill 0:867c4eabf128 165 pc.printf("\r\nOut of Range\r\n");
ibestwill 0:867c4eabf128 166 }
ibestwill 0:867c4eabf128 167 else {
ibestwill 0:867c4eabf128 168 pc.printf("Distance = %2f", distance);
ibestwill 0:867c4eabf128 169 wait(1);
ibestwill 0:867c4eabf128 170 }
ibestwill 0:867c4eabf128 171 wait(0.5);
ibestwill 0:867c4eabf128 172 } while(!pc.readable());
ibestwill 0:867c4eabf128 173 }
ibestwill 0:867c4eabf128 174
ibestwill 0:867c4eabf128 175 void RcServoMotor(void)
ibestwill 0:867c4eabf128 176 {
ibestwill 0:867c4eabf128 177 unsigned int V, W, X, n;
ibestwill 0:867c4eabf128 178 DE0.format(16,1); // SPI is mode 1
ibestwill 0:867c4eabf128 179 DE0.frequency(500000); // SPI frequency
ibestwill 0:867c4eabf128 180 IoReset=0; // Reset all SPI peripherals
ibestwill 0:867c4eabf128 181 IoReset=1;
ibestwill 0:867c4eabf128 182 wait_us(5);
ibestwill 0:867c4eabf128 183 IoReset=0;
ibestwill 0:867c4eabf128 184
ibestwill 0:867c4eabf128 185 SpiReset=0;
ibestwill 0:867c4eabf128 186 SpiReset=1;
ibestwill 0:867c4eabf128 187 wait_us(5);
ibestwill 0:867c4eabf128 188 SpiReset=0;
ibestwill 0:867c4eabf128 189 V=DE0.write(0x0402); // Control word to write RC Servo pulse interval in ms.
ibestwill 0:867c4eabf128 190 //n=4000;
ibestwill 0:867c4eabf128 191
ibestwill 0:867c4eabf128 192 do {
ibestwill 0:867c4eabf128 193 /*
ibestwill 0:867c4eabf128 194 wait_us(10);
ibestwill 0:867c4eabf128 195 W=DE0.write(0x10); // Write RC Servo pulse interval in ms
ibestwill 0:867c4eabf128 196 X=DE0.write(n); // Write position to RC Servo M1.
ibestwill 0:867c4eabf128 197 n=n+2000;
ibestwill 0:867c4eabf128 198 if(n==60000) n=4000;
ibestwill 0:867c4eabf128 199 pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
ibestwill 0:867c4eabf128 200 wait(1);
ibestwill 0:867c4eabf128 201
ibestwill 0:867c4eabf128 202 */
ibestwill 0:867c4eabf128 203
ibestwill 0:867c4eabf128 204 for(n=0;n<=60000;n=n+6000){
ibestwill 0:867c4eabf128 205 wait_us(10);
ibestwill 0:867c4eabf128 206 W=DE0.write(0x10); // Write RC Servo pulse interval in ms
ibestwill 0:867c4eabf128 207 X=DE0.write(n); // Write position to RC Servo M1.
ibestwill 0:867c4eabf128 208 pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
ibestwill 0:867c4eabf128 209 wait(1);
ibestwill 0:867c4eabf128 210 }
ibestwill 0:867c4eabf128 211
ibestwill 0:867c4eabf128 212 for(n=60000;n>=6000;n=n-6000){
ibestwill 0:867c4eabf128 213 wait_us(10);
ibestwill 0:867c4eabf128 214 W=DE0.write(0x10); // Write RC Servo pulse interval in ms
ibestwill 0:867c4eabf128 215 X=DE0.write(n); // Write position to RC Servo M1.
ibestwill 0:867c4eabf128 216 pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
ibestwill 0:867c4eabf128 217 wait(1);
ibestwill 0:867c4eabf128 218 }
ibestwill 0:867c4eabf128 219
ibestwill 0:867c4eabf128 220 } while(!pc.readable());
ibestwill 0:867c4eabf128 221
ibestwill 0:867c4eabf128 222 }
ibestwill 0:867c4eabf128 223
ibestwill 0:867c4eabf128 224
ibestwill 1:f1d9b55f7dd7 225
ibestwill 0:867c4eabf128 226 //**************************************************************************
ibestwill 1:f1d9b55f7dd7 227 void Userinterface(void){
ibestwill 1:f1d9b55f7dd7 228
ibestwill 1:f1d9b55f7dd7 229
ibestwill 1:f1d9b55f7dd7 230 char dirkey;
ibestwill 1:f1d9b55f7dd7 231
ibestwill 1:f1d9b55f7dd7 232 pc.printf("\n\rEnter -W- -A- -S- -D- for Manual Control:\n ");
ibestwill 1:f1d9b55f7dd7 233 pc.scanf("%c",&dirkey);
ibestwill 1:f1d9b55f7dd7 234 pc.printf("\n\rKey Entered:%c\n\n\r",dirkey);
ibestwill 1:f1d9b55f7dd7 235
ibestwill 1:f1d9b55f7dd7 236
ibestwill 1:f1d9b55f7dd7 237 //Forward
ibestwill 1:f1d9b55f7dd7 238 if(dirkey=='w'||dirkey=='W') {
ibestwill 1:f1d9b55f7dd7 239
ibestwill 1:f1d9b55f7dd7 240 if(Setspeed_R<35) {
ibestwill 1:f1d9b55f7dd7 241 Setspeed_R=Setspeed_R+5;
ibestwill 1:f1d9b55f7dd7 242 } else Setspeed_R=35;
ibestwill 0:867c4eabf128 243
ibestwill 1:f1d9b55f7dd7 244 if(Setspeed_L>-35) {
ibestwill 1:f1d9b55f7dd7 245 Setspeed_L=Setspeed_L-5;
ibestwill 1:f1d9b55f7dd7 246 } else Setspeed_L=-35;
ibestwill 1:f1d9b55f7dd7 247 }
ibestwill 1:f1d9b55f7dd7 248
ibestwill 1:f1d9b55f7dd7 249
ibestwill 1:f1d9b55f7dd7 250
ibestwill 1:f1d9b55f7dd7 251 //Backward
ibestwill 1:f1d9b55f7dd7 252 else if (dirkey=='s'||dirkey=='S') {
ibestwill 1:f1d9b55f7dd7 253
ibestwill 1:f1d9b55f7dd7 254 if(Setspeed_R>-35) {
ibestwill 1:f1d9b55f7dd7 255 Setspeed_R=Setspeed_R-5;
ibestwill 1:f1d9b55f7dd7 256 } else Setspeed_R=-35;
ibestwill 1:f1d9b55f7dd7 257
ibestwill 1:f1d9b55f7dd7 258 if(Setspeed_L<35) {
ibestwill 1:f1d9b55f7dd7 259 Setspeed_L=Setspeed_L+5;
ibestwill 1:f1d9b55f7dd7 260 } else Setspeed_L=35;
ibestwill 1:f1d9b55f7dd7 261 }
ibestwill 1:f1d9b55f7dd7 262
ibestwill 1:f1d9b55f7dd7 263 //Left Turn
ibestwill 1:f1d9b55f7dd7 264 else if (dirkey=='a'||dirkey=='A') {
ibestwill 0:867c4eabf128 265
ibestwill 1:f1d9b55f7dd7 266 if(Setspeed_R<35) {
ibestwill 1:f1d9b55f7dd7 267 Setspeed_R=Setspeed_R+2;
ibestwill 1:f1d9b55f7dd7 268 } else Setspeed_R=35;
ibestwill 1:f1d9b55f7dd7 269
ibestwill 1:f1d9b55f7dd7 270 if(Setspeed_L>-35) {
ibestwill 1:f1d9b55f7dd7 271 Setspeed_L=Setspeed_L-1;
ibestwill 1:f1d9b55f7dd7 272 } else Setspeed_L=-35;
ibestwill 1:f1d9b55f7dd7 273
ibestwill 1:f1d9b55f7dd7 274 }
ibestwill 1:f1d9b55f7dd7 275
ibestwill 1:f1d9b55f7dd7 276 //Right Turn
ibestwill 1:f1d9b55f7dd7 277 else if (dirkey=='d'||dirkey=='D') {
ibestwill 1:f1d9b55f7dd7 278
ibestwill 1:f1d9b55f7dd7 279 if(Setspeed_R<35) {
ibestwill 1:f1d9b55f7dd7 280 Setspeed_R=Setspeed_R+1;
ibestwill 1:f1d9b55f7dd7 281 } else Setspeed_R=35;
ibestwill 1:f1d9b55f7dd7 282
ibestwill 1:f1d9b55f7dd7 283 if(Setspeed_L>-35) {
ibestwill 1:f1d9b55f7dd7 284 Setspeed_L=Setspeed_L-2;
ibestwill 1:f1d9b55f7dd7 285 } else Setspeed_L=-35;
ibestwill 0:867c4eabf128 286
ibestwill 1:f1d9b55f7dd7 287 }
ibestwill 1:f1d9b55f7dd7 288
ibestwill 1:f1d9b55f7dd7 289 //Stop
ibestwill 1:f1d9b55f7dd7 290 else if (dirkey=='x'||dirkey=='X') {
ibestwill 1:f1d9b55f7dd7 291
ibestwill 1:f1d9b55f7dd7 292 Setspeed_R=0;
ibestwill 1:f1d9b55f7dd7 293 y_R=0;
ibestwill 1:f1d9b55f7dd7 294
ibestwill 1:f1d9b55f7dd7 295 Setspeed_L=0;
ibestwill 1:f1d9b55f7dd7 296 y_L=0;
ibestwill 1:f1d9b55f7dd7 297
ibestwill 1:f1d9b55f7dd7 298 }
ibestwill 1:f1d9b55f7dd7 299
ibestwill 1:f1d9b55f7dd7 300 //Wrong Key Entered
ibestwill 1:f1d9b55f7dd7 301 else pc.printf("\n\rInvalid Key!\n\r ");
ibestwill 1:f1d9b55f7dd7 302
ibestwill 1:f1d9b55f7dd7 303
ibestwill 1:f1d9b55f7dd7 304
ibestwill 1:f1d9b55f7dd7 305 }
ibestwill 1:f1d9b55f7dd7 306
ibestwill 1:f1d9b55f7dd7 307
ibestwill 1:f1d9b55f7dd7 308
ibestwill 0:867c4eabf128 309
ibestwill 0:867c4eabf128 310 //**************************************************************************
ibestwill 0:867c4eabf128 311 void Initialize(void)
ibestwill 0:867c4eabf128 312 {
ibestwill 0:867c4eabf128 313 //Baud Rate
ibestwill 0:867c4eabf128 314 //pc.baud (460800);
ibestwill 0:867c4eabf128 315
ibestwill 0:867c4eabf128 316 //Intialization Motor
ibestwill 0:867c4eabf128 317 PwmR.period_us(T);
ibestwill 0:867c4eabf128 318 PwmL.period_us(T);
ibestwill 0:867c4eabf128 319
ibestwill 0:867c4eabf128 320
ibestwill 0:867c4eabf128 321 //Initialization QEI - to be exected once(normally)
ibestwill 0:867c4eabf128 322 DE0.format(16,1); //SPI format:16-bit words, mode 1 protocol or mode 0????????????
ibestwill 0:867c4eabf128 323 //DE0.frequency(500000);
ibestwill 0:867c4eabf128 324 IoReset=0;
ibestwill 0:867c4eabf128 325 IoReset=1;
ibestwill 0:867c4eabf128 326 IoReset=0;
ibestwill 0:867c4eabf128 327
ibestwill 0:867c4eabf128 328 SpiReset=0;
ibestwill 0:867c4eabf128 329 SpiReset=1;
ibestwill 0:867c4eabf128 330 wait_us(10);
ibestwill 0:867c4eabf128 331 SpiReset=0;
ibestwill 1:f1d9b55f7dd7 332 int ID = DE0.write(0x8004); //SPI slave control word to read (only) 2-word transactions starting at base address 0 within the peripheral
ibestwill 0:867c4eabf128 333
ibestwill 0:867c4eabf128 334 if(ID == 23) printf("\r\n\nSPI Interface is Successful ! ID = %d\r\n\n",ID); //ID=0x0017
ibestwill 0:867c4eabf128 335 else printf("\r\n********SPI Interface failed!********\r\n\n");
ibestwill 0:867c4eabf128 336
ibestwill 0:867c4eabf128 337
ibestwill 0:867c4eabf128 338 }
ibestwill 0:867c4eabf128 339
ibestwill 0:867c4eabf128 340
ibestwill 0:867c4eabf128 341
ibestwill 0:867c4eabf128 342 //**************************************************************************
ibestwill 0:867c4eabf128 343
ibestwill 0:867c4eabf128 344 void PIController(void){
ibestwill 0:867c4eabf128 345
ibestwill 0:867c4eabf128 346 dPos_R = DE0.write(0x00);
ibestwill 0:867c4eabf128 347 dTime_R = DE0.write(0x00);
ibestwill 1:f1d9b55f7dd7 348
ibestwill 1:f1d9b55f7dd7 349 dPos_L = DE0.write(0x00);
ibestwill 1:f1d9b55f7dd7 350 dTime_L = DE0.write(0x00);
ibestwill 0:867c4eabf128 351
ibestwill 0:867c4eabf128 352 dPos_R = Signextend(dPos_R);
ibestwill 1:f1d9b55f7dd7 353 dPos_L = Signextend(dPos_L);
ibestwill 1:f1d9b55f7dd7 354
ibestwill 0:867c4eabf128 355
ibestwill 1:f1d9b55f7dd7 356 Vel_R = (123*dPos_R)/dTime_R; //radians per second
ibestwill 1:f1d9b55f7dd7 357 Vel_L = (123*dPos_L)/dTime_L; //radians per second
ibestwill 0:867c4eabf128 358
ibestwill 0:867c4eabf128 359
ibestwill 0:867c4eabf128 360 //Error
ibestwill 0:867c4eabf128 361 e_R = SatSub(Setspeed_R,Vel_R);
ibestwill 1:f1d9b55f7dd7 362 e_R = SatValue(e_R,70);
ibestwill 1:f1d9b55f7dd7 363
ibestwill 1:f1d9b55f7dd7 364 e_L = SatSub(Setspeed_L,Vel_L);
ibestwill 1:f1d9b55f7dd7 365 e_L = SatValue(e_L,70);
ibestwill 1:f1d9b55f7dd7 366
ibestwill 0:867c4eabf128 367
ibestwill 0:867c4eabf128 368
ibestwill 0:867c4eabf128 369 //xtate
ibestwill 0:867c4eabf128 370 xT_R = SatAdd(x_R,e_R);
ibestwill 1:f1d9b55f7dd7 371 //xT_R = SatValue(x_R,320000);
ibestwill 0:867c4eabf128 372
ibestwill 1:f1d9b55f7dd7 373 xT_L = SatAdd(x_L,e_L);
ibestwill 1:f1d9b55f7dd7 374 //xT_L = SatValue(x_L,320000);
ibestwill 0:867c4eabf128 375
ibestwill 0:867c4eabf128 376 //proportional and integral control terms
ibestwill 0:867c4eabf128 377 uP_R=(Kp_R*e_R);
ibestwill 0:867c4eabf128 378 uI_R=(Ki_R*x_R);
ibestwill 0:867c4eabf128 379
ibestwill 1:f1d9b55f7dd7 380 uP_L=(Kp_L*e_L);
ibestwill 1:f1d9b55f7dd7 381 uI_L=(Ki_L*x_L);
ibestwill 1:f1d9b55f7dd7 382
ibestwill 0:867c4eabf128 383 uS_R = SatAdd(uP_R,uI_R);
ibestwill 1:f1d9b55f7dd7 384 u_R = SatValue(uS_R,35);
ibestwill 1:f1d9b55f7dd7 385
ibestwill 1:f1d9b55f7dd7 386 uS_L = SatAdd(uP_L,uI_L);
ibestwill 1:f1d9b55f7dd7 387 u_L = SatValue(uS_L,35);
ibestwill 0:867c4eabf128 388
ibestwill 0:867c4eabf128 389 if (u_R==uS_R) x_R = xT_R; //Integrator windup prevention
ibestwill 1:f1d9b55f7dd7 390 if (u_L==uS_L) x_L = xT_L; //Integrator windup prevention
ibestwill 1:f1d9b55f7dd7 391
ibestwill 0:867c4eabf128 392
ibestwill 0:867c4eabf128 393 if(u_R >= 0) DirR = 1; else DirR = 0;
ibestwill 1:f1d9b55f7dd7 394 if(u_L >= 0) DirL = 0; else DirL = 1;
ibestwill 0:867c4eabf128 395
ibestwill 1:f1d9b55f7dd7 396 y_R=u_R/35.0*T;
ibestwill 0:867c4eabf128 397 PwmR.pulsewidth_us(abs(y_R));
ibestwill 0:867c4eabf128 398
ibestwill 1:f1d9b55f7dd7 399 y_L=u_L/35.0*T;
ibestwill 1:f1d9b55f7dd7 400 PwmL.pulsewidth_us(abs(y_L));
ibestwill 1:f1d9b55f7dd7 401
ibestwill 0:867c4eabf128 402
ibestwill 0:867c4eabf128 403 }
ibestwill 0:867c4eabf128 404
ibestwill 0:867c4eabf128 405
ibestwill 0:867c4eabf128 406 //**************************************************************************
ibestwill 0:867c4eabf128 407
ibestwill 1:f1d9b55f7dd7 408
ibestwill 0:867c4eabf128 409 int SatAdd(int x, int y)
ibestwill 0:867c4eabf128 410 {
ibestwill 0:867c4eabf128 411 // Detect overflow and saturate result
ibestwill 0:867c4eabf128 412 int z;
ibestwill 0:867c4eabf128 413 z = x + y;
ibestwill 0:867c4eabf128 414 if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
ibestwill 0:867c4eabf128 415 else if ((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
ibestwill 0:867c4eabf128 416 return z;
ibestwill 0:867c4eabf128 417 }
ibestwill 0:867c4eabf128 418
ibestwill 0:867c4eabf128 419
ibestwill 0:867c4eabf128 420 int SatSub(int x, int y)
ibestwill 0:867c4eabf128 421 {
ibestwill 0:867c4eabf128 422 int z;
ibestwill 0:867c4eabf128 423 z = x - y;
ibestwill 0:867c4eabf128 424 // 32-bit overflow detection and saturating arithmetic
ibestwill 0:867c4eabf128 425 if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
ibestwill 0:867c4eabf128 426 else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
ibestwill 0:867c4eabf128 427 return z;
ibestwill 0:867c4eabf128 428 }
ibestwill 0:867c4eabf128 429
ibestwill 0:867c4eabf128 430
ibestwill 0:867c4eabf128 431 int SatValue(int x, int Limit)
ibestwill 0:867c4eabf128 432 {
ibestwill 0:867c4eabf128 433 // Impose maximum limit on integrator state
ibestwill 0:867c4eabf128 434 if(x > Limit) return(Limit);
ibestwill 0:867c4eabf128 435 else if(x < -Limit) return(-Limit);
ibestwill 0:867c4eabf128 436 else return(x);
ibestwill 0:867c4eabf128 437 }
ibestwill 0:867c4eabf128 438
ibestwill 0:867c4eabf128 439
ibestwill 0:867c4eabf128 440 int Signextend(int x) {
ibestwill 0:867c4eabf128 441 if (x & 0x00008000) {
ibestwill 0:867c4eabf128 442 x = x | 0xFFFF0000;
ibestwill 0:867c4eabf128 443 }
ibestwill 0:867c4eabf128 444 return x;
ibestwill 0:867c4eabf128 445 }
ibestwill 0:867c4eabf128 446
ibestwill 1:f1d9b55f7dd7 447
ibestwill 0:867c4eabf128 448 //**************************************************************************
ibestwill 0:867c4eabf128 449 //**************************************************************************
ibestwill 0:867c4eabf128 450
ibestwill 0:867c4eabf128 451
ibestwill 0:867c4eabf128 452 // ******** Control Thread ********
ibestwill 0:867c4eabf128 453 void PiControlThread(void const *argument)
ibestwill 0:867c4eabf128 454 {
ibestwill 0:867c4eabf128 455 while (true) {
ibestwill 0:867c4eabf128 456 osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received.
ibestwill 0:867c4eabf128 457 led2= !led2; // Alive status - led2 toggles each time PiControlThread is signaled.
ibestwill 0:867c4eabf128 458
ibestwill 0:867c4eabf128 459 PIController();
ibestwill 0:867c4eabf128 460
ibestwill 0:867c4eabf128 461 }
ibestwill 0:867c4eabf128 462 }
ibestwill 0:867c4eabf128 463
ibestwill 0:867c4eabf128 464 // ******** Collision Thread ********
ibestwill 0:867c4eabf128 465 void ExtCollisionThread(void const *argument)
ibestwill 0:867c4eabf128 466 {
ibestwill 0:867c4eabf128 467 while (true) {
ibestwill 0:867c4eabf128 468 osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received
ibestwill 0:867c4eabf128 469 led4 = 1;
ibestwill 0:867c4eabf128 470 }
ibestwill 0:867c4eabf128 471 }
ibestwill 0:867c4eabf128 472
ibestwill 0:867c4eabf128 473 // ******** Watchdog Interrupt Handler ********
ibestwill 0:867c4eabf128 474 void Watchdog(void const *n)
ibestwill 0:867c4eabf128 475 {
ibestwill 0:867c4eabf128 476 led3=1; // led3 is activated when the watchdog timer times out
ibestwill 0:867c4eabf128 477 }
ibestwill 0:867c4eabf128 478
ibestwill 0:867c4eabf128 479 // ******** Period Timer Interrupt Handler ********
ibestwill 0:867c4eabf128 480 void PiControllerISR(void)
ibestwill 0:867c4eabf128 481 {
ibestwill 0:867c4eabf128 482 osSignalSet(PiControl,0x1); // Activate the signal, PiControl, with each periodic timer interrupt.
ibestwill 0:867c4eabf128 483
ibestwill 0:867c4eabf128 484 }
ibestwill 0:867c4eabf128 485
ibestwill 0:867c4eabf128 486 // ******** Collision Interrupt Handler ********
ibestwill 0:867c4eabf128 487 void ExtCollisionISR(void)
ibestwill 0:867c4eabf128 488 {
ibestwill 0:867c4eabf128 489 osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt.
ibestwill 0:867c4eabf128 490 }
ibestwill 0:867c4eabf128 491
ibestwill 0:867c4eabf128 492 //**************************************************************************