Diff: Lab3.cpp

Revision:

0:b3cd4463d972

Child:

2:82e4eac97f0a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Lab3.cpp	Sat Feb 13 23:23:08 2016 +0000
@@ -0,0 +1,279 @@
+// EE4333 Robotics Lab 3
+
+// Library Imports
+
+//#include "InterruptIn.h"
+//#include "rtos.h"
+#include "mbed.h"
+#include "Serial.h"
+#include "stdio.h"
+
+// Function Declarations
+
+void PiControllerISR(void);
+void WdtFaultISR(void);
+void ExtCollisionISR(void);
+void PiControlThread(void const *argument);
+void ExtCollisionThread(void const *argument);
+void Watchdog(void const *n);
+
+void UserInput(void);
+void DE0_Init(int);
+void L_MotorInit(void);
+void R_MotorInit(void);
+
+// ********************************************************************
+//                     GLOBAL VARIABLE DECLARATIONS
+// ********************************************************************
+
+    signed int setpoint;                // Desired Angular Speed ( rad/sec )
+    signed int e;                       // Velocity Error
+    signed int u;                       // Control Signal
+    signed int integrator;              // Integrator State
+
+// *********************************************************************
+//                     PROCESSES AND THREADS
+// *********************************************************************
+
+// Processes and threads
+//int32_t SignalPi, SignalWdt, SignalExtCollision; //Semaphores
+/*
+osThreadId PiControl,WdtFault,ExtCollision;
+osThreadDef(PiControlThread, osPriorityRealtime, DEFAULT_STACK_SIZE); // Declare Control as a thread/process
+osThreadDef(ExtCollisionThread, osPriorityHigh, DEFAULT_STACK_SIZE); // Declare External Collision as a thread/process
+osTimerDef(Wdtimer, Watchdog); // Declare a watch dog timer
+//typedef enum{ */
+    
+    // Interpretation based on rtos.h 
+    
+    // osPriorityIdle = -3,                 //< priority: idle (lowest)
+    // osPriorityLow = -2,                  //< priority: low
+    // osPriorityBelowNormal = -1,          //< priority: below normal
+    // osPriorityNormal = 0,                //< priority: normal (default)
+    // osPriorityAboveNormal = +1,          //< priority: above normal
+    // osPriorityHigh = +2,                 //< priority: high
+    // osPriorityRealtime = +3,             //< priority: realtime (highest)
+    // osPriority;
+
+
+
+
+// *********************************************************************
+//                          PIN DECLARATIONS
+// *********************************************************************
+
+// Digital I/O Pins
+
+    DigitalOut led1(LED1);              // Thread Indicators
+    DigitalOut led2(LED2);              //
+    DigitalOut led3(LED3);              //
+    DigitalOut led4(LED4);              //
+    
+    DigitalOut DirL(p29);               // Direction of Left Motor
+    DigitalOut DirR(p30);               // Direction of Right Motor
+
+    // SPI Related Digital I/O Pins
+    
+    DigitalOut SpiReset(p11);
+    DigitalOut IoReset(p12);
+
+//PWM
+
+    PwmOut PwmL(p22);
+    PwmOut PwmR(p21);
+
+//Serial
+
+    Serial pc(USBTX, USBRX);            // tx and rx for PC serial channel via USB cable
+    Serial Bluetooth(p9,p10);     // Pins tx(p9) , rx(p10) for bluetooth serial channel
+
+
+//SPI
+
+    SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
+
+    
+//Interrupts
+
+    //InterruptIn Bumper(p8);           // External interrupt pin declared as Bumper
+    Ticker ControlInterrupt;            // Internal Interrupt to trigger Control Thread
+
+// =====================================================================
+// =====================================================================
+
+
+// *********************************************************************
+//                        MAIN FUNCTION
+// *********************************************************************
+
+int main(){
+   
+   // Initialization
+   
+   DE0_Init(0x8002);                            // Initializes DEO to Mode 1, Varifies Correct Peripheral ID
+                                                // Takes in control word to configure SPI settings                                                                         
+   L_MotorInit();                               // Enables Pwm and sets direction bits for left motor
+   R_MotorInit();                               // Enables Pwm and sets direction bits for right motor
+   integrator = 0;                              // Initializes integrator state to zero ( Global Variable )
+   
+   // Read Inputs from Console
+  
+    UserInput();
+   
+   // Display Global Variables to Console
+   
+   while(1){
+       
+       printf("\n\r ****************************************************");
+       printf("\n\r User Setpoint    : %i",setpoint);      // User defined setpoint
+       printf("\n\r Velocity Error   : %i",e);             // Displays signed Velocity Error
+       printf("\n\r Integrator State : %i",integrator);    // Displays currently state of integrator
+       printf("\n\r Control Signal   : %i",u);             // Displays control signal
+       printf("\n\r ****************************************************");
+       printf("\n\r\n\r");
+       wait(1);
+       
+       // Echo to bluetooth later
+       
+   }
+   
+}
+
+
+// ***************************************************
+//                      DE0 Init
+// ***************************************************
+
+void DE0_Init(int SpiControlWord){
+    
+    int mode = 1;
+    int bits = 16;
+   
+    DE0.format(bits,mode);
+    
+    // Verify Peripheral ID
+    
+    // Generates single square pulse to reset DE0 IO
+    
+    IoReset = 0;
+    IoReset = 1;
+    IoReset = 0;
+    
+    // Generates single square pulse to reset DE0 SPI
+    
+    SpiReset = 0;
+    SpiReset = 1;
+    SpiReset = 0;
+    
+    // Writes to DE0 Control Register
+    
+    int ID = DE0.write(SpiControlWord);             // SPI Control Word specifies SPI settings
+    
+    if(ID == 23){                                   // DE0 ID 23 (0x0017)
+        printf("\n\r >> DE0 Initialized.\n\r");}
+    else{
+        printf("\n\r >> Failed to initialize DE0 board.\n\r");}
+}
+
+// ***************************************************
+//              Left Motor Initialization
+// ***************************************************
+
+    // Pwm Pin Left Motor : p21
+    // Direction Pin Left Motor : p29
+
+void L_MotorInit(void){
+
+   DirL = 1;                                   // Defaults to 0.
+
+    // Direction bit logic output
+    // 0 : Backwards ( Reverse ) 
+    // 1 : Forwards  ( Advance )
+
+    PwmL.period_us(100);
+    PwmL.pulsewidth_us(0);
+    
+}
+    
+// ***************************************************
+//             Right Motor Initialization
+// ***************************************************
+
+    // Pwm Pin Right Motor : p22
+    // Direction Pin Right Motor : p30
+    
+void R_MotorInit(void){
+    
+   DirR = 1;                                   // Defaults to 0.
+    
+    // Direction bit logic output
+    // 0 : Backwards ( Reverse ) 
+    // 1 : Forwards  ( Advance )
+    
+    PwmR.period_us(100);
+    PwmR.pulsewidth_us(0);
+    
+}
+    
+/// ***************************************************
+//                 User Input
+// ***************************************************
+
+void UserInput(){
+   
+   Bluetooth.printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
+   printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
+   
+   if(Bluetooth.readable()){     
+       Bluetooth.scanf("%i",&setpoint);
+       printf("\n\r << Input received via bluetooth >>");}
+   else{
+       scanf("%i",&setpoint);
+       Bluetooth.printf("\n\r << Input received via pc console >>");}
+       
+   printf("\n\r Your number was >> %i\n\r",setpoint);
+   Bluetooth.printf("\n\r Your number was >> %i\n\r",setpoint);
+   
+   wait_ms(500);
+   
+   // Update later to accept period of Pwm ( maybe )
+
+}
+
+/*
+
+// ******** Control Thread ********
+
+void PiControlThread(void const *argument)
+{
+    while (true) {
+        osSignalWait(SignalPi, osWaitForever); // Go to sleep until, SignalPi, is received.
+        led2 = !led2; // Alive status - led2 toggles each time PiControlThread is signaled.
+        printf("\n\r PI Control Working");
+        if(u >= 0) {
+            if(u>T) {
+                printf("\n\r Maximum Exceeded. Limiting to 1.0 Duty Cycle");
+                u = T;   //Overflow protection
+            }
+            //PW1.pulsewidth_us(u);
+            DIR1 = 1;
+            printf("\n\r Direction: CW");
+        } else if(u < 0) {
+            if(u<-T) {
+                printf("\n\r Maximum Exceeded. Limiting to 1.0 Duty Cycle");
+                u = T;   //Overflow protection
+            }
+            //PW1.pulsewidth_us(u);
+            DIR1 = 0;
+            u = -u;
+            printf("\n\r Selected Pulse Width = %d us", u);
+            printf("\n\r Direction: CCW");
+        }
+        PW1.pulsewidth_us(u);
+        Position = Position + 1;
+        printf("\n\r Duty Cycle = %0.6f", PW1.read());
+        osSignalSet(PiControl, 0x000);
+    }
+}
+
+*/