File content as of revision 2:7e4930f48021:

#include "algo.h"
#include "fmea.h"
#include "DAC.h"
#include "Mux.h"

// - - - Parameters - - - //

//car parameters
const float trackwidth = 49*0.0254;
const float wheelradius = 0.254;
const float rear_wheelbase = 0.686816;                                   //rear weightbias*wheelbase

//constant parameters
const float full_throttle=5;
const float dead_steering=7;
const float integral_saturation=0.5;
const float dead_rpm = 10;

// - - - Inputs - - - //

//steering, throttle inputs
float steering,steering2;                                                         //input steering wheel angle in degrees
float throttle, throttle2;                                                         //throttle input from pedal
float yaw_rate;

// - - - Outputs - - - //

// throttle output values
float throttle_Left, throttle_Right;

// - - - For Basic Algo and Steering to turnradius function - - - //

//Ticker for algo interrupt
Ticker for_algo;

//algo function, PID funtion
void ediff_func();
void pid(void);

//delta_Left, delta_Right, turnradius and wratio_desired calculated realtime from delta (delta_Left and delta_Right are Left and Right tyre angles)
float delta_Left;                                                       //Left wheel Steer angle
float delta_Right;                                                      //Right wheel Steer angle
float turnradius;
float wratio_desired;                                                   //Right/Left

//Openloop Flag
volatile bool openloop_lowrpm = true;
volatile bool openloop_driver = false;
const float rpm_openloop_limit = 100;

//PID constants
float kp=0;
float kd=0;
float ki=0;
float c=0;

//For PID
const float timeint = 0.1;
float integral;
float derivative;
float error_prev = 0;     //error at t0
float error_new;          //error at t
float wratio_actual;
float w_ratio;            //ratio of controller voltages - Right/Left

// - - - For RPM Measurement - - - //

//Left and Right rear RPMs feedback-input 
float rpm_Left;
float rpm_Right;
float rpm_FrontRight;
float rpm_FrontLeft;

// - - - - FMEA OUTPUT - - - - //
bool shutdown = false;
volatile int fmea_switch = 0;
Ticker for_FmeaCall;
void FmeaCall();

//////////////////////////////////////CAN DATA TO DAQ////////////////////////////////////////////
CAN CANtoDAQ(p9, p10);  //Check PIN Numbers;
Ticker SendDataToDAQ;
char CANdataSet1[8];
char CANdataSet2[8];

// add motor temperatures and motor currents
void PopulateData(void)
{
    CANdataSet1[0] = (char)((steering + 110) /220 * 255);
    CANdataSet1[1] = (char)(throttle/5 * 255);
    CANdataSet1[2] = (char)(((uint16_t)wratio_actual) >> 8);           //Check range later. 
    CANdataSet1[3] = (char)(((uint16_t)wratio_actual));                //For now assumed to be 16 bit 0 - 65535
    CANdataSet1[4] = (char)((wratio_desired - 0.5) / 1.4);
    CANdataSet1[5] = (char)(throttle_Left/5 * 255);
    CANdataSet1[6] = (char)(throttle_Right/5 * 255);
    CANdataSet1[7] = 0;    
    
    CANdataSet2[0] = (char)((steering2 + 110) /220 * 255);
    CANdataSet2[1] = (char)(throttle2/5 * 255);
    CANdataSet2[2] = (char)(rpm_Left/1000 * 255);
    CANdataSet2[3] = (char)(rpm_Right/1000 * 255);
    CANdataSet2[4] = (char)(rpm_FrontLeft/1000 * 255);
    CANdataSet2[5] = (char)(rpm_FrontRight/1000 * 255);
    CANdataSet2[6] = (char)(yaw_rate/5 * 255);                         //Decide range later. For now 0 to 5.
    CANdataSet2[7] = 0;    
}

void SendData(void)
{
    PopulateData();
    CANtoDAQ.write(CANMessage(1000, CANdataSet1, 8));   
    CANtoDAQ.write(CANMessage(1001, CANdataSet2, 8));
}

///////////////////////////////RECEIVE SPI DATA FROM ICAP//////////////////////////////////////////
SPI spi(p5,p6,p7);
DigitalOut cs(p8);
#define SPI_BYTE_ARRAY_SIZE 13
uint8_t SPIDataFromICAP[SPI_BYTE_ARRAY_SIZE];
uint8_t startByte, endByte;
int SPIByteCounter = 0;
bool SPIError = false;
Ticker PullDataFromICAP;

void UpdateData()
{
    startByte = SPIDataFromICAP[0];
    endByte = SPIDataFromICAP[12];
    if (((char)startByte != '@') || ((char)endByte != '#'))
    {
        //Some Problem with SPI.
        SPIError = true;
    }
    
    /*    
    steering = SPIDataFromICAP[1];
    steering2 = SPIDataFromICAP[2];
    throttle = SPIDataFromICAP[3];
    throttle2 = SPIDataFromICAP[4];
    brake = SPIDataFromICAP[5];
    brake2 = SPIDataFromICAP[6];
    rpm_Left = SPIDataFromICAP[7];
    rpm_Right = SPIDataFromICAP[8];
    rpm_FrontLeft = SPIDataFromICAP[9];
    rpm_FrontRight = SPIDataFromICAP[10];
    flagsFromICAP = SPIDataFromICAP[11];    
    */    
}

void SPIPullData()
{
    SPIByteCounter = 0;
    while(SPIByteCounter < SPI_BYTE_ARRAY_SIZE)
    {
        cs=0;
        spi.write(0xFF);
        SPIDataFromICAP[SPIByteCounter] = spi.write(0xAA);
        //pc.printf("%u",SPIDataFromICAP[SPIByteCounter]);
        SPIByteCounter++;        
        cs = 1;
        wait(0.0001);
    }
    UpdateData();
}

///////////////////////////////////////MUX///////////////////////////////////
unsigned short currentLeftMux;
unsigned short currentRightMux;
unsigned short tempLeftMux;
unsigned short tempRightMux;
//unsigned short brakeLeftMux;
//unsigned short brakeRightMux;
unsigned short throttleLeftMux;
unsigned short throttleRightMux;

int Mux_switch; 
Ticker for_Mux; 

void MuxCall()
{
    switch (Mux_switch)
    {
        case 0: 
        readFromMux(throttleLeftMuxSelect, throttleLeftMux);
        break;
        case 1: 
        readFromMux(throttleRightMuxSelect, throttleRightMux);
        break;
        case 2: 
        readFromMux(tempLeftMuxSelect, tempLeftMux);
        break;
        case 3: 
        readFromMux(tempRightMuxSelect, tempRightMux);
        break;
        case 4: 
        readFromMux(currentLeftMuxSelect, currentLeftMux);
        break;
        case 5: 
        readFromMux(currentRightMuxSelect, currentRightMux);
        break;
    }
    Mux_switch++;
    if (Mux_switch == 6)
    {
        Mux_switch = 0;
    }    
}

//////////////////////////////////DAC/////////////////////////////////////////
Ticker for_DAC;
int DAC_switch; 
 
void outputToDAC()
{
    switch (DAC_switch)
    {       
        case 0: 
        writeToDAC(throttleLeftDACChannel, (unsigned short) (throttle_Left * 65535/5) , WRITE_TO_INPUT_REGISTER_AND_UPDATE_DAC_REGISTER);
        break;
        case 1: 
        writeToDAC(throttleRightDACChannel, (unsigned short) (throttle_Right * 65535/5) , WRITE_TO_INPUT_REGISTER_AND_UPDATE_DAC_REGISTER);
        break;
    }
    DAC_switch++;
    if (DAC_switch == 2)
    {
        DAC_switch = 0;
    }
}

////////////////////////////////////////////////////MAIN////////////////////////////////////////////////////
int main()
{   
    for_FmeaCall.attach(&FmeaCall,1);
    for_algo.attach_us(&ediff_func,(algoperiod));                   //call ediff func every "algoperiod" sec  
    SendDataToDAQ.attach(&SendData, 0.05);
    
    //DAC in Main
    initDAC();
    for_DAC.attach(&outputToDAC, 0.01);    
    
    //MUX in Main
    for_Mux.attach(&MuxCall, 0.5);
    
    cs = 1;
    spi.format(8,3);
    spi.frequency(1000000);
    PullDataFromICAP.attach(&SPIPullData, 0.02);
    spi.write(0xAA);    

    while(1)
    {
        //Take steering 1,2 and throttle 1,2 from Input capture
    }    
}

//Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
void ediff_func()    
{        
    //call pid function which gives v1, v2 output    
    pid();                  
    
    //send throttle_Left and throttle_Right to Kelly Controllers 
}

void FmeaCall()
{
    switch (fmea_switch)
    {
        case 0: 
        throttle_Left_pulldown_fmea();
        break;
        case 1: 
        throttle_Right_pulldown_fmea();
        break;
        case 2: 
        throttle_comparison_fmea();
        break;
        case 3: 
        steering2_pulldown_fmea();
        break;
        case 4: 
        steering_comparison_fmea();
        break;
        case 5: 
        mux_feedback_fmea();
        break;
    }
    fmea_switch++;
    if (fmea_switch == 6)
    {
        fmea_switch = 0;
    }    
}