ECE 4333 / Mbed 2 deprecated Lab3

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

Lab3.cpp@0:b3cd4463d972, 2016-02-13 (annotated)

Committer:
A_Sterner
Date:
Sat Feb 13 23:23:08 2016 +0000
Revision:
0:b3cd4463d972
Child:
2:82e4eac97f0a
Lab 3 : Working SPI, Bluetooth; - Need encoder, control thread

Who changed what in which revision?

UserRevisionLine numberNew contents of line
A_Sterner 0:b3cd4463d972 1 // EE4333 Robotics Lab 3
A_Sterner 0:b3cd4463d972 2
A_Sterner 0:b3cd4463d972 3 // Library Imports
A_Sterner 0:b3cd4463d972 4
A_Sterner 0:b3cd4463d972 5 //#include "InterruptIn.h"
A_Sterner 0:b3cd4463d972 6 //#include "rtos.h"
A_Sterner 0:b3cd4463d972 7 #include "mbed.h"
A_Sterner 0:b3cd4463d972 8 #include "Serial.h"
A_Sterner 0:b3cd4463d972 9 #include "stdio.h"
A_Sterner 0:b3cd4463d972 10
A_Sterner 0:b3cd4463d972 11 // Function Declarations
A_Sterner 0:b3cd4463d972 12
A_Sterner 0:b3cd4463d972 13 void PiControllerISR(void);
A_Sterner 0:b3cd4463d972 14 void WdtFaultISR(void);
A_Sterner 0:b3cd4463d972 15 void ExtCollisionISR(void);
A_Sterner 0:b3cd4463d972 16 void PiControlThread(void const *argument);
A_Sterner 0:b3cd4463d972 17 void ExtCollisionThread(void const *argument);
A_Sterner 0:b3cd4463d972 18 void Watchdog(void const *n);
A_Sterner 0:b3cd4463d972 19
A_Sterner 0:b3cd4463d972 20 void UserInput(void);
A_Sterner 0:b3cd4463d972 21 void DE0_Init(int);
A_Sterner 0:b3cd4463d972 22 void L_MotorInit(void);
A_Sterner 0:b3cd4463d972 23 void R_MotorInit(void);
A_Sterner 0:b3cd4463d972 24
A_Sterner 0:b3cd4463d972 25 // ********************************************************************
A_Sterner 0:b3cd4463d972 26 // GLOBAL VARIABLE DECLARATIONS
A_Sterner 0:b3cd4463d972 27 // ********************************************************************
A_Sterner 0:b3cd4463d972 28
A_Sterner 0:b3cd4463d972 29 signed int setpoint; // Desired Angular Speed ( rad/sec )
A_Sterner 0:b3cd4463d972 30 signed int e; // Velocity Error
A_Sterner 0:b3cd4463d972 31 signed int u; // Control Signal
A_Sterner 0:b3cd4463d972 32 signed int integrator; // Integrator State
A_Sterner 0:b3cd4463d972 33
A_Sterner 0:b3cd4463d972 34 // *********************************************************************
A_Sterner 0:b3cd4463d972 35 // PROCESSES AND THREADS
A_Sterner 0:b3cd4463d972 36 // *********************************************************************
A_Sterner 0:b3cd4463d972 37
A_Sterner 0:b3cd4463d972 38 // Processes and threads
A_Sterner 0:b3cd4463d972 39 //int32_t SignalPi, SignalWdt, SignalExtCollision; //Semaphores
A_Sterner 0:b3cd4463d972 40 /*
A_Sterner 0:b3cd4463d972 41 osThreadId PiControl,WdtFault,ExtCollision;
A_Sterner 0:b3cd4463d972 42 osThreadDef(PiControlThread, osPriorityRealtime, DEFAULT_STACK_SIZE); // Declare Control as a thread/process
A_Sterner 0:b3cd4463d972 43 osThreadDef(ExtCollisionThread, osPriorityHigh, DEFAULT_STACK_SIZE); // Declare External Collision as a thread/process
A_Sterner 0:b3cd4463d972 44 osTimerDef(Wdtimer, Watchdog); // Declare a watch dog timer
A_Sterner 0:b3cd4463d972 45 //typedef enum{ */
A_Sterner 0:b3cd4463d972 46
A_Sterner 0:b3cd4463d972 47 // Interpretation based on rtos.h
A_Sterner 0:b3cd4463d972 48
A_Sterner 0:b3cd4463d972 49 // osPriorityIdle = -3, //< priority: idle (lowest)
A_Sterner 0:b3cd4463d972 50 // osPriorityLow = -2, //< priority: low
A_Sterner 0:b3cd4463d972 51 // osPriorityBelowNormal = -1, //< priority: below normal
A_Sterner 0:b3cd4463d972 52 // osPriorityNormal = 0, //< priority: normal (default)
A_Sterner 0:b3cd4463d972 53 // osPriorityAboveNormal = +1, //< priority: above normal
A_Sterner 0:b3cd4463d972 54 // osPriorityHigh = +2, //< priority: high
A_Sterner 0:b3cd4463d972 55 // osPriorityRealtime = +3, //< priority: realtime (highest)
A_Sterner 0:b3cd4463d972 56 // osPriority;
A_Sterner 0:b3cd4463d972 57
A_Sterner 0:b3cd4463d972 58
A_Sterner 0:b3cd4463d972 59
A_Sterner 0:b3cd4463d972 60
A_Sterner 0:b3cd4463d972 61 // *********************************************************************
A_Sterner 0:b3cd4463d972 62 // PIN DECLARATIONS
A_Sterner 0:b3cd4463d972 63 // *********************************************************************
A_Sterner 0:b3cd4463d972 64
A_Sterner 0:b3cd4463d972 65 // Digital I/O Pins
A_Sterner 0:b3cd4463d972 66
A_Sterner 0:b3cd4463d972 67 DigitalOut led1(LED1); // Thread Indicators
A_Sterner 0:b3cd4463d972 68 DigitalOut led2(LED2); //
A_Sterner 0:b3cd4463d972 69 DigitalOut led3(LED3); //
A_Sterner 0:b3cd4463d972 70 DigitalOut led4(LED4); //
A_Sterner 0:b3cd4463d972 71
A_Sterner 0:b3cd4463d972 72 DigitalOut DirL(p29); // Direction of Left Motor
A_Sterner 0:b3cd4463d972 73 DigitalOut DirR(p30); // Direction of Right Motor
A_Sterner 0:b3cd4463d972 74
A_Sterner 0:b3cd4463d972 75 // SPI Related Digital I/O Pins
A_Sterner 0:b3cd4463d972 76
A_Sterner 0:b3cd4463d972 77 DigitalOut SpiReset(p11);
A_Sterner 0:b3cd4463d972 78 DigitalOut IoReset(p12);
A_Sterner 0:b3cd4463d972 79
A_Sterner 0:b3cd4463d972 80 //PWM
A_Sterner 0:b3cd4463d972 81
A_Sterner 0:b3cd4463d972 82 PwmOut PwmL(p22);
A_Sterner 0:b3cd4463d972 83 PwmOut PwmR(p21);
A_Sterner 0:b3cd4463d972 84
A_Sterner 0:b3cd4463d972 85 //Serial
A_Sterner 0:b3cd4463d972 86
A_Sterner 0:b3cd4463d972 87 Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable
A_Sterner 0:b3cd4463d972 88 Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel
A_Sterner 0:b3cd4463d972 89
A_Sterner 0:b3cd4463d972 90
A_Sterner 0:b3cd4463d972 91 //SPI
A_Sterner 0:b3cd4463d972 92
A_Sterner 0:b3cd4463d972 93 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
A_Sterner 0:b3cd4463d972 94
A_Sterner 0:b3cd4463d972 95
A_Sterner 0:b3cd4463d972 96 //Interrupts
A_Sterner 0:b3cd4463d972 97
A_Sterner 0:b3cd4463d972 98 //InterruptIn Bumper(p8); // External interrupt pin declared as Bumper
A_Sterner 0:b3cd4463d972 99 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
A_Sterner 0:b3cd4463d972 100
A_Sterner 0:b3cd4463d972 101 // =====================================================================
A_Sterner 0:b3cd4463d972 102 // =====================================================================
A_Sterner 0:b3cd4463d972 103
A_Sterner 0:b3cd4463d972 104
A_Sterner 0:b3cd4463d972 105 // *********************************************************************
A_Sterner 0:b3cd4463d972 106 // MAIN FUNCTION
A_Sterner 0:b3cd4463d972 107 // *********************************************************************
A_Sterner 0:b3cd4463d972 108
A_Sterner 0:b3cd4463d972 109 int main(){
A_Sterner 0:b3cd4463d972 110
A_Sterner 0:b3cd4463d972 111 // Initialization
A_Sterner 0:b3cd4463d972 112
A_Sterner 0:b3cd4463d972 113 DE0_Init(0x8002); // Initializes DEO to Mode 1, Varifies Correct Peripheral ID
A_Sterner 0:b3cd4463d972 114 // Takes in control word to configure SPI settings
A_Sterner 0:b3cd4463d972 115 L_MotorInit(); // Enables Pwm and sets direction bits for left motor
A_Sterner 0:b3cd4463d972 116 R_MotorInit(); // Enables Pwm and sets direction bits for right motor
A_Sterner 0:b3cd4463d972 117 integrator = 0; // Initializes integrator state to zero ( Global Variable )
A_Sterner 0:b3cd4463d972 118
A_Sterner 0:b3cd4463d972 119 // Read Inputs from Console
A_Sterner 0:b3cd4463d972 120
A_Sterner 0:b3cd4463d972 121 UserInput();
A_Sterner 0:b3cd4463d972 122
A_Sterner 0:b3cd4463d972 123 // Display Global Variables to Console
A_Sterner 0:b3cd4463d972 124
A_Sterner 0:b3cd4463d972 125 while(1){
A_Sterner 0:b3cd4463d972 126
A_Sterner 0:b3cd4463d972 127 printf("\n\r ****************************************************");
A_Sterner 0:b3cd4463d972 128 printf("\n\r User Setpoint : %i",setpoint); // User defined setpoint
A_Sterner 0:b3cd4463d972 129 printf("\n\r Velocity Error : %i",e); // Displays signed Velocity Error
A_Sterner 0:b3cd4463d972 130 printf("\n\r Integrator State : %i",integrator); // Displays currently state of integrator
A_Sterner 0:b3cd4463d972 131 printf("\n\r Control Signal : %i",u); // Displays control signal
A_Sterner 0:b3cd4463d972 132 printf("\n\r ****************************************************");
A_Sterner 0:b3cd4463d972 133 printf("\n\r\n\r");
A_Sterner 0:b3cd4463d972 134 wait(1);
A_Sterner 0:b3cd4463d972 135
A_Sterner 0:b3cd4463d972 136 // Echo to bluetooth later
A_Sterner 0:b3cd4463d972 137
A_Sterner 0:b3cd4463d972 138 }
A_Sterner 0:b3cd4463d972 139
A_Sterner 0:b3cd4463d972 140 }
A_Sterner 0:b3cd4463d972 141
A_Sterner 0:b3cd4463d972 142
A_Sterner 0:b3cd4463d972 143 // ***************************************************
A_Sterner 0:b3cd4463d972 144 // DE0 Init
A_Sterner 0:b3cd4463d972 145 // ***************************************************
A_Sterner 0:b3cd4463d972 146
A_Sterner 0:b3cd4463d972 147 void DE0_Init(int SpiControlWord){
A_Sterner 0:b3cd4463d972 148
A_Sterner 0:b3cd4463d972 149 int mode = 1;
A_Sterner 0:b3cd4463d972 150 int bits = 16;
A_Sterner 0:b3cd4463d972 151
A_Sterner 0:b3cd4463d972 152 DE0.format(bits,mode);
A_Sterner 0:b3cd4463d972 153
A_Sterner 0:b3cd4463d972 154 // Verify Peripheral ID
A_Sterner 0:b3cd4463d972 155
A_Sterner 0:b3cd4463d972 156 // Generates single square pulse to reset DE0 IO
A_Sterner 0:b3cd4463d972 157
A_Sterner 0:b3cd4463d972 158 IoReset = 0;
A_Sterner 0:b3cd4463d972 159 IoReset = 1;
A_Sterner 0:b3cd4463d972 160 IoReset = 0;
A_Sterner 0:b3cd4463d972 161
A_Sterner 0:b3cd4463d972 162 // Generates single square pulse to reset DE0 SPI
A_Sterner 0:b3cd4463d972 163
A_Sterner 0:b3cd4463d972 164 SpiReset = 0;
A_Sterner 0:b3cd4463d972 165 SpiReset = 1;
A_Sterner 0:b3cd4463d972 166 SpiReset = 0;
A_Sterner 0:b3cd4463d972 167
A_Sterner 0:b3cd4463d972 168 // Writes to DE0 Control Register
A_Sterner 0:b3cd4463d972 169
A_Sterner 0:b3cd4463d972 170 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
A_Sterner 0:b3cd4463d972 171
A_Sterner 0:b3cd4463d972 172 if(ID == 23){ // DE0 ID 23 (0x0017)
A_Sterner 0:b3cd4463d972 173 printf("\n\r >> DE0 Initialized.\n\r");}
A_Sterner 0:b3cd4463d972 174 else{
A_Sterner 0:b3cd4463d972 175 printf("\n\r >> Failed to initialize DE0 board.\n\r");}
A_Sterner 0:b3cd4463d972 176 }
A_Sterner 0:b3cd4463d972 177
A_Sterner 0:b3cd4463d972 178 // ***************************************************
A_Sterner 0:b3cd4463d972 179 // Left Motor Initialization
A_Sterner 0:b3cd4463d972 180 // ***************************************************
A_Sterner 0:b3cd4463d972 181
A_Sterner 0:b3cd4463d972 182 // Pwm Pin Left Motor : p21
A_Sterner 0:b3cd4463d972 183 // Direction Pin Left Motor : p29
A_Sterner 0:b3cd4463d972 184
A_Sterner 0:b3cd4463d972 185 void L_MotorInit(void){
A_Sterner 0:b3cd4463d972 186
A_Sterner 0:b3cd4463d972 187 DirL = 1; // Defaults to 0.
A_Sterner 0:b3cd4463d972 188
A_Sterner 0:b3cd4463d972 189 // Direction bit logic output
A_Sterner 0:b3cd4463d972 190 // 0 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 191 // 1 : Forwards ( Advance )
A_Sterner 0:b3cd4463d972 192
A_Sterner 0:b3cd4463d972 193 PwmL.period_us(100);
A_Sterner 0:b3cd4463d972 194 PwmL.pulsewidth_us(0);
A_Sterner 0:b3cd4463d972 195
A_Sterner 0:b3cd4463d972 196 }
A_Sterner 0:b3cd4463d972 197
A_Sterner 0:b3cd4463d972 198 // ***************************************************
A_Sterner 0:b3cd4463d972 199 // Right Motor Initialization
A_Sterner 0:b3cd4463d972 200 // ***************************************************
A_Sterner 0:b3cd4463d972 201
A_Sterner 0:b3cd4463d972 202 // Pwm Pin Right Motor : p22
A_Sterner 0:b3cd4463d972 203 // Direction Pin Right Motor : p30
A_Sterner 0:b3cd4463d972 204
A_Sterner 0:b3cd4463d972 205 void R_MotorInit(void){
A_Sterner 0:b3cd4463d972 206
A_Sterner 0:b3cd4463d972 207 DirR = 1; // Defaults to 0.
A_Sterner 0:b3cd4463d972 208
A_Sterner 0:b3cd4463d972 209 // Direction bit logic output
A_Sterner 0:b3cd4463d972 210 // 0 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 211 // 1 : Forwards ( Advance )
A_Sterner 0:b3cd4463d972 212
A_Sterner 0:b3cd4463d972 213 PwmR.period_us(100);
A_Sterner 0:b3cd4463d972 214 PwmR.pulsewidth_us(0);
A_Sterner 0:b3cd4463d972 215
A_Sterner 0:b3cd4463d972 216 }
A_Sterner 0:b3cd4463d972 217
A_Sterner 0:b3cd4463d972 218 /// ***************************************************
A_Sterner 0:b3cd4463d972 219 // User Input
A_Sterner 0:b3cd4463d972 220 // ***************************************************
A_Sterner 0:b3cd4463d972 221
A_Sterner 0:b3cd4463d972 222 void UserInput(){
A_Sterner 0:b3cd4463d972 223
A_Sterner 0:b3cd4463d972 224 Bluetooth.printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
A_Sterner 0:b3cd4463d972 225 printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
A_Sterner 0:b3cd4463d972 226
A_Sterner 0:b3cd4463d972 227 if(Bluetooth.readable()){
A_Sterner 0:b3cd4463d972 228 Bluetooth.scanf("%i",&setpoint);
A_Sterner 0:b3cd4463d972 229 printf("\n\r << Input received via bluetooth >>");}
A_Sterner 0:b3cd4463d972 230 else{
A_Sterner 0:b3cd4463d972 231 scanf("%i",&setpoint);
A_Sterner 0:b3cd4463d972 232 Bluetooth.printf("\n\r << Input received via pc console >>");}
A_Sterner 0:b3cd4463d972 233
A_Sterner 0:b3cd4463d972 234 printf("\n\r Your number was >> %i\n\r",setpoint);
A_Sterner 0:b3cd4463d972 235 Bluetooth.printf("\n\r Your number was >> %i\n\r",setpoint);
A_Sterner 0:b3cd4463d972 236
A_Sterner 0:b3cd4463d972 237 wait_ms(500);
A_Sterner 0:b3cd4463d972 238
A_Sterner 0:b3cd4463d972 239 // Update later to accept period of Pwm ( maybe )
A_Sterner 0:b3cd4463d972 240
A_Sterner 0:b3cd4463d972 241 }
A_Sterner 0:b3cd4463d972 242
A_Sterner 0:b3cd4463d972 243 /*
A_Sterner 0:b3cd4463d972 244
A_Sterner 0:b3cd4463d972 245 // ******** Control Thread ********
A_Sterner 0:b3cd4463d972 246
A_Sterner 0:b3cd4463d972 247 void PiControlThread(void const *argument)
A_Sterner 0:b3cd4463d972 248 {
A_Sterner 0:b3cd4463d972 249 while (true) {
A_Sterner 0:b3cd4463d972 250 osSignalWait(SignalPi, osWaitForever); // Go to sleep until, SignalPi, is received.
A_Sterner 0:b3cd4463d972 251 led2 = !led2; // Alive status - led2 toggles each time PiControlThread is signaled.
A_Sterner 0:b3cd4463d972 252 printf("\n\r PI Control Working");
A_Sterner 0:b3cd4463d972 253 if(u >= 0) {
A_Sterner 0:b3cd4463d972 254 if(u>T) {
A_Sterner 0:b3cd4463d972 255 printf("\n\r Maximum Exceeded. Limiting to 1.0 Duty Cycle");
A_Sterner 0:b3cd4463d972 256 u = T; //Overflow protection
A_Sterner 0:b3cd4463d972 257 }
A_Sterner 0:b3cd4463d972 258 //PW1.pulsewidth_us(u);
A_Sterner 0:b3cd4463d972 259 DIR1 = 1;
A_Sterner 0:b3cd4463d972 260 printf("\n\r Direction: CW");
A_Sterner 0:b3cd4463d972 261 } else if(u < 0) {
A_Sterner 0:b3cd4463d972 262 if(u<-T) {
A_Sterner 0:b3cd4463d972 263 printf("\n\r Maximum Exceeded. Limiting to 1.0 Duty Cycle");
A_Sterner 0:b3cd4463d972 264 u = T; //Overflow protection
A_Sterner 0:b3cd4463d972 265 }
A_Sterner 0:b3cd4463d972 266 //PW1.pulsewidth_us(u);
A_Sterner 0:b3cd4463d972 267 DIR1 = 0;
A_Sterner 0:b3cd4463d972 268 u = -u;
A_Sterner 0:b3cd4463d972 269 printf("\n\r Selected Pulse Width = %d us", u);
A_Sterner 0:b3cd4463d972 270 printf("\n\r Direction: CCW");
A_Sterner 0:b3cd4463d972 271 }
A_Sterner 0:b3cd4463d972 272 PW1.pulsewidth_us(u);
A_Sterner 0:b3cd4463d972 273 Position = Position + 1;
A_Sterner 0:b3cd4463d972 274 printf("\n\r Duty Cycle = %0.6f", PW1.read());
A_Sterner 0:b3cd4463d972 275 osSignalSet(PiControl, 0x000);
A_Sterner 0:b3cd4463d972 276 }
A_Sterner 0:b3cd4463d972 277 }
A_Sterner 0:b3cd4463d972 278
A_Sterner 0:b3cd4463d972 279 */