File content as of revision 0:b3cd4463d972:

// 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);
    }
}

*/