gistfile1.cpp

/*

This program is meant for an Arduino Mega 2560 and is used as the control system
for the 2012 Jayhawk Motorsports Electric Car.

Author: Aric Beaver
Date: 4/06/2012
Version: 2
Specifications: This is the second version of this code that uses the throttle
sensor, brake sensor, accelerometer, wheel speed sensors, CANBUS system for
vehicle control commands, and data aquistion system.

Setup:
1) Setup libraries
2) Setup up global variables
        a) Toggles for various sensors processes
        b) Pin declarations
3) Setup CANBUS system
4) Setup data aquisition system
5) Enable RMS inverters via CANBUS

Process:
1) Read throttle sensor
2) Read brake sensors
3) Calculate torque commands for electronic differential
                a) Calculate torque to each wheel based on throttle input, lateral vehicle
                         accleration, and wheel velocity/acceleration
                b) Calculate braking to each wheel based on throttle input, lateral vehicle
                         accleration, and wheel velocity/acceleration
                NOTE: braking commands are the inverse of torque commands
4) Apply signals for command over CANBUS system (0x0C0 and 0x1C0)
                a) D1_Torque_Command, 0-15 bits, signed
                b) D3_Direction_Command, 32 bit, unsigned, 0=CW and 1=CCW
                c) D4_Inverter_Enable, 32 bit, unsigned, 0=Off and 1=On
                etc...
5) Data aquistion
                a) Header "time,throttle_voltage,brake_front_voltage,brake_rear_voltage,D1_Torque_Command"
                b) Write data in order of header

*/

// Libraries
// =============================================================================
#include "Wire.h"               // Allows direct pin interaction
#include "SPI.h"                // Abstracts SPI for comm with the CAN Controller
#include "MCP2515.h"            // Abstraction for handling CAN

// Constants
// =============================================================================
// PLEASE READ THE RMS MANUAL
//   This enum corresponds to the 16 addresses used for messages
//   broadcast by an RMS motor cotroller, based on the base address
//   of the controller in question.
//   These are used for capturing logging data of some kind directly from MC.
typedef enum
{
    TEMP1,
    TEMP2,
    TEMP3_SHUDDER,
    ANALOG_IN_VOLTAGES,
    DIGITAL_IN_STATUS,
    MOTOR_POSITION,
    CURR_INFO,
    VOLTAGE_INFO,
    FLUX_INFO,
    INTERNAL_VOLTAGES,
    INTERNAL_STATES,
    FAULT_CODES,
    TORQUE_TIMER_INFO,
    MOD_IDX_FLUX_OUT_INFO,
    FIRMWARE_INFO,
    DIAG_DATA
} brdcast_msg_id;

// CAN
#define MC_CAN_RX_ADDR      0x00A0  // DEC 160 -- default base broadcast address
#define MC_CAN_TX_ADDR      0x00C0  // DEC 192 -- default command address
const int CANBUS_kBaud    = 1000;   // Define kBaud for CAN bus

// Pins
// =============================================================================
// Cable select pin for SPI bus
#define CS_PIN      53  
// Regular interrupt pin from CAN Shield. Triggers that there is a message in
// the CAN buffer
#define INT_PIN     2   
// Analog Pin 4 being used as digital. Tells Shutdown that inverters are ready
#define OK_PIN      A4  
// First throttle
#define TPS_PIN     0   

// Timer Settings
#define TIME_1_MASK         ( 1 << OCIE1A );  // Timer Compare Interrupt
#define TIME_1_REG_B        ( 1 << WGM12 ) | ( 1 << CS12 ) | ( 1 << CS10 )
//      Run in CTC Mode, Prescaler set to 1024

#define TIME_1_PRESCALE     10
#define TIME_1_SEC_INT      F_CPU >> TIME_1_PRESCALE
#define TIME_1_SEC_DIV      1
#define TIME_1_NUM_SECONDS  1 << TIME_1_SEC_DIV
#define TIME_1_INTERVAL     TIME_1_SEC_INT * TIME_1_NUM_SECONDS

// Settings allowing you to turn on and off different features
// =============================================================================
#define LOGGING    0
// This setting controls the amount of serial output that is performed.
// 0 = No output at all
// 1 = Output major logging steps and detected faults when activating motor controllers
// 2 = Dump all CAN data while trying to activate motor controller. VERY VERBOSE.
#define SERIAL     1
#if SERIAL > 0
    #define SERIAL_PRINT(x) Serial.print(x)
	#define SERIAL_PRINTN(x, y) Serial.print(x, y)
    #define SERIAL_PRINTLN(x) Serial.println(x)
	#define SERIAL_PRINTLNN(x, y) Serial.println(x, y)
#else
    #define SERIAL_PRINT(x)
	#define SERIAL_PRINTN(x, y)
    #define SERIAL_PRINTLN(x)
	#define SERIAL_PRINTLNN(x, y)
#endif

// Global variables
// =============================================================================

// Toggle for various sensors and processes
int tps_toggle = 1;
int canbus_toggle = 1;

// CANBUS shield and MCP2515 startup
// Pin definitions specific to how the MCP2515 is wired up.
MCP2515 CAN( CS_PIN, INT_PIN ); // Create CAN object with pins as defined
// CAN message frame (actually just the parts that are exposed by the MCP2515 RX/TX buffers)
byte i = 0;
Frame message; // Updates every iteration of "loop" and in the "setup"

// Count the number of successfully transmitted torque requests.
volatile unsigned int   transmit_counter;
volatile unsigned int   transmit_last_cnt;
volatile unsigned int   transmit_per_sec;
volatile bool           transmit_ready;

// Function prototypes
// =============================================================================
void read_CANBUS();
bool writeCANBUS();
int over_under_travel_TPS( int canbus_toggle, int tps_pin );
void inverters_ready( int msg_rx_addr, int msg_tx_addr );

// Setup function to define I/O and turn on the system
// =============================================================================
void setup() {

    // Enable serial communication if we're in debugging mode
#if SERIAL > 0
    Serial.begin(9600);
#endif

    // Pin input and output delcarations
    // =============================================================================
    // Required for CANBUS shield hack 10,11,12,13 --> 53,51,50,52
    pinMode(53, OUTPUT); 

    // Set out throttle pin to be input
    pinMode(TPS_PIN, INPUT);
    // Set our signal to the shutdown board to be output..
    pinMode(OK_PIN, OUTPUT);

#if LOGGING > 0
    // Setup interrupt for timed debug messages
    // =============================================================================
    noInterrupts();
    TCCR1A = 0;
    TCCR1B = 0;
    TCNT1 = 0;

    OCR1A = TIME_1_INTERVAL;
    TCCR1B |= TIME_1_REG_B;
    TIMSK1 |= TIME_1_MASK;
    interrupts();
#endif

    // Initialize CAN bus system
    // =============================================================================
    SERIAL_PRINT("CANBUS: Initializing MCP2515... ");
    // Set up SPI Communication
    // dataMode can be SPI_MODE0 or SPI_MODE3 only for MCP2515
    SPI.setClockDivider(SPI_CLOCK_DIV2);
    SPI.setDataMode(SPI_MODE0);
    SPI.setBitOrder(MSBFIRST);
    SPI.begin();
    // Initialise MCP2515 CAN controller at the specified speed and clock frequency
    // In this case 125kbps with a 16MHz oscillator
    // (Note:  This is the oscillator attached to the MCP2515, not the Arduino oscillaltor)
    int baudRate = CAN.Init(CANBUS_kBaud, 16);
    if (baudRate > 0) {
        SERIAL_PRINTLN("OK.");
        SERIAL_PRINT("  Baud Rate (kbps): ");
        SERIAL_PRINTLNN(baudRate, DEC);
    } else {
        SERIAL_PRINTLN("Failed.");
    }

    // Wait for the inverters to become ready, i.e. VSM State = 4
    SERIAL_PRINTLN("CANBUS: Waiting for inverters to be ready... ");
    // Pass in the base address for the motor controller we're checking
    inverters_ready(MC_CAN_RX_ADDR, MC_CAN_TX_ADDR);
    // Inverters are now ready since the function returned. This means time to
    // start the HV. Send final signal to shutdown board
    digitalWrite(OK_PIN, HIGH);

}

ISR( TIMER1_COMPA_vect )
{
    // Calculate the transmissions / second
    transmit_per_sec = ( transmit_counter - transmit_last_cnt ) >> TIME_1_SEC_DIV;

    // Update previous count / time, and flag as ready.
    transmit_last_cnt = transmit_counter;
    transmit_ready = true;
}


// =============================================================================
// Main loop to call measurement and control and functions
// =============================================================================

void loop() {

    // Obtain measurements
    // =============================================================================
    // Throttle position
    // The sensor ouputs a high voltage when not extended and low when extended, therefore,
    // it should be extended when static for 0 throttle.
    int TPS = analogRead(TPS_PIN);

    // Calculate torque commands
    // =============================================================================
    // Interpolate between torque values and throttle position
    // 16 bits signed (-32767 to +32767)*0.1 = -3276 to +3276 factor
    // Maximum throttle input is 900 and minimum is 100 with 10-bit Arduino ADC
    // Torque that the EMRAX motors can handle is 200/130 Nm peak/cont
    int raw_torque_command;
    if ((100 <= TPS) && (TPS <= 900)) {
        // Find raw torque value by "inverting" the TPS reading because 900
        // corresponds to minimum throttle while 100 is maximum throttle
        // raw_torque_command = abs((TPS-100)*(3276/800)-3476);
        raw_torque_command = (TPS-100)*(2000/800);

    } else if ((0 <= TPS) && (TPS < 100)) {
        // Throttle is at minimum, set torque commands to minimum value
        raw_torque_command = 0;
    } else if ((900 <       TPS) && (TPS <= 1024)) {
        // Throttle is at idle, set torque commands to minimum value
        raw_torque_command = 2000;
    } else {
        // 1) TPS signal is out of range
        // 2) Shut off torque commands temporarily
        // 3) Exit to external function and wait for TPS signal to return to
        //        the valid operation range between 0 and 1024
        // 4) Set torque commands to zero when the throttle returns to valid range
        //canbus_toggle = 0;
        //canbus_toggle = over_under_travel_TPS(canbus_toggle, TPS_PIN);
        raw_torque_command = 0;
    }

    // Send CANBUS torque commands on 0x0C0 and 0x1C0
    // =============================================================================
    // Convert decimal number to two bytes
    int byte0, byte1;

    if (raw_torque_command < 0) {
        byte1 = floor(abs(raw_torque_command) / 256);
        byte0 = raw_torque_command - 256 * byte1;
        byte1 = byte1 + 128;
    } else {
        byte1 = floor(raw_torque_command / 256);
        byte0 = raw_torque_command - 256 * byte1;
    }

    // Broadcast torque command, byte 0 (low byte) and byte 1 (high byte)
    message.id = MC_CAN_TX_ADDR;
    message.dlc = 8;
    message.data[0] = byte0;
    message.data[1] = byte1;
    message.data[2] = 0;
    message.data[3] = 0;
    message.data[4] = 0;
    if (byte0 + byte1 == 0) {
        message.data[5] = 0;   // Disable inverter if 0 Nm
    }
    else {
        message.data[5] = 1;
    }
    message.data[6] = 0;
    message.data[7] = 0;

#if LOGGING > 0
    if (writeCANBUS())
        ++transmit_counter;

    if (transmit_ready) {
        SERIAL_PRINT( "Transmissions / sec: " );
        SERIAL_PRINTLNN( transmit_per_sec, DEC );
        transmit_ready = false;
    }

    // Check for any messages broadcast via CAN
    read_CANBUS();
#else
    writeCANBUS();
#endif
}

void read_CANBUS()
{
    if( CAN.Interrupt() )
    {
        byte interruptFlags = CAN.Read(CANINTF);
        if(interruptFlags & RX0IF) {
            message = CAN.ReadBuffer(RXB0);
        }
        if(interruptFlags & RX1IF) {
            message = CAN.ReadBuffer(RXB1);
        }

        // ASSUMPTIONS:
        //  1.  Base address for the controller takes the form 0xXX0
        //  2.  Thus address range is assumed to be 0xXX0 - 0xXXF
        //  3.  Data chunks are sent little endian
        //  4.  The platform this code is run on is little endian
        //
        // TODO: More robust handling.
        if( MC_CAN_RX_ADDR != ( message.id & ( ~0x0FUL ) ) )
            return;

        //  Each brodcast address corresponds to a different message.
        brdcast_msg_id msg = (brdcast_msg_id)( message.id & 0x0FUL );
        switch( msg )
        {
        case TEMP1:
        {
            short * val = (short *)message.data;
            SERIAL_PRINTLN( "Module Temperatures:" );

            SERIAL_PRINT( "\tIGBT Module A: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tIGBT Module B: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tIGBT Module C: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tGate Driver: " );
            SERIAL_PRINTLNN( *val, DEC );
        }
        break;

        case TEMP2:
        {
            short * val = (short *)message.data;
            SERIAL_PRINTLN( "Module Temperatures:" );

            SERIAL_PRINT( "\tControl Board: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tRTD #1: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tRTD #2: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tRTD #3: " );
            SERIAL_PRINTLNN( *val, DEC );
        }
        break;

        case TEMP3_SHUDDER:
        {
            short * val = (short *)message.data;
            SERIAL_PRINTLN( "Temp / Torque Shudder:" );

            SERIAL_PRINT( "\tRTD #4 Temp: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tRTD #5 Temp: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tMotor Temp: " );
            SERIAL_PRINTLNN( *val, DEC );
            ++val;

            SERIAL_PRINT( "\tTorque Shudder: " );
            SERIAL_PRINTLNN( *val, DEC );
        }
        break;

        default:
        {
            unsigned long * msg = (unsigned long *)( message.data );

            SERIAL_PRINT( "DIAG_MSG{" );
            SERIAL_PRINTN( message.id, HEX );
            SERIAL_PRINT( "} : VAL: " );
            SERIAL_PRINTLNN( *msg, HEX );
        }
        break;
        }
    }
}

// Exernal functions for control, measurement, and setup
// =============================================================================

// function writeCANBUS()
// This function writes the CANBUS global 'message' to the bus. Then
// promptly clears the 'message' to refrain from bad messages being written.
bool writeCANBUS()
{
    if( 0 == canbus_toggle )
        return false;

    bool status = CAN.Interrupt();
    if( status )
    {
        CAN.LoadBuffer( TXB0, message );
        CAN.SendBuffer( TXB0 );
    }

    // Clear the message to refrain from writing bad messages
    message.id = 0;
    message.data[0] = 0;
    message.data[1] = 0;
    message.data[2] = 0;
    message.data[3] = 0;
    message.data[4] = 0;
    message.data[5] = 0;
    message.data[6] = 0;
    message.data[7] = 0;

    return status;
}

// function over_under_travel_TPS()
// This function is only called when the throttle reading exceeds the
// intended maximum or minimum. It will wait until the throttle reading
// returns to a valid operation range between 0 to 1024
int over_under_travel_TPS(int canbus_toggle, int tps_pin) {

    // Initially read TPS pin
    int TPS = analogRead(TPS_PIN);

    // Enter loop that will return the canbus toggle to a valid state
    while(1) {
        if (0 <= TPS <= 1024) {
            return 1;
        } else {
            // Reread TPS pin and continue loop
            TPS = analogRead(TPS_PIN);
            SERIAL_PRINT("TPS over travel reading: ");
            SERIAL_PRINTLN(TPS);
        }
    }

}

// function invertersReady()
// This function is called to check if motor controllers are ready for HV
// and subsequenctly torque commands. This function will not
// return unless the inverters are ready for torque commands, or "enabled".
void inverters_ready(int msg_rx_addr, int msg_tx_addr) {
    // Calculate our message ID by adding different amounts to the base
    unsigned int internal_msg = msg_rx_addr + (unsigned int)INTERNAL_STATES;
    unsigned int fault_msg = msg_rx_addr + (unsigned int)FAULT_CODES;

    // Now loop endlessly until we get what we'd like
    int enabled = 0;
    while (!enabled) {
        // If there's a message to recieve
        if (CAN.Interrupt()) {

            // Read CAN bus. Grab upper and lower words of data
            byte interruptFlags = CAN.Read(CANINTF);
            if (interruptFlags & RX0IF)
                message = CAN.ReadBuffer(RXB0);
            if (interruptFlags & RX1IF)
                message = CAN.ReadBuffer(RXB1);

            // If we've recieved an internal state identifier...
            if (message.id == internal_msg) {
                // What is the state telling us?
                if (message.data[0] == 4) {
                    // Inverter seems to be ready so broadcast: disable, enable,
                    // disable messages, check motor controller response, then exit
                    message.data[5] = 0;
                    message.id = msg_tx_addr;
                    SERIAL_PRINTLN("Sending disable command to MC.");
                    writeCANBUS();
                    message.data[5] = 1;
                    message.id = msg_tx_addr;
                    SERIAL_PRINTLN("Sending enable command to MC.");
                    writeCANBUS();

                    SERIAL_PRINT("Checking for response to the toggle (each dot is a message from CAN)");

                    // Now loop until we recieved an internal state message
                    while (message.id != internal_msg) {
                        if (CAN.Interrupt()) {
                            SERIAL_PRINT(".");
                            // Read CAN bus. Grab upper and lower words of data
                            byte interruptFlags = CAN.Read(CANINTF);
                            if (interruptFlags & RX0IF)
                                message = CAN.ReadBuffer(RXB0);
                            if (interruptFlags & RX1IF)
                                message = CAN.ReadBuffer(RXB1);
                        }
                    }
                    if (message.data[6] == 1) {
                        SERIAL_PRINTLN(" enabled!");
                        enabled = 1;
                    } else {
                        SERIAL_PRINTLN(" not enabled :(");
                        enabled = 0;
                    }

                    message.data[5] = 0;
                    message.id = msg_tx_addr;
                    SERIAL_PRINTLN("Sending final disable command.");
                    writeCANBUS();

                } else {
                    // VSM state skipped wait state, most likely in VSM blink mode
                    // because the CAN message was lost. Check inverter faults...
                    SERIAL_PRINT("VSM state = ");
                    SERIAL_PRINTLNN(message.data[0], DEC);
                }
            } else if (message.id == fault_msg) {
#if SERIAL > 0
                SERIAL_PRINTLN("Motor controller transmitted a fault while waiting for ready internal state: ");
                for (int i = 0; i < message.dlc; i++) {
                    Serial.write(message.data[i]);
                    SERIAL_PRINT(" ");
                }
                SERIAL_PRINTLN("END");
#endif
            } else {
#if SERIAL > 1
                SERIAL_PRINTLN("Motor controller transmitted something while waiting for ready internal state: ");
                SERIAL_PRINTN(message.id, DEC);
                SERIAL_PRINT("=");
                for (int i = 0; i < message.dlc; i++) {
                    Serial.write(message.data[i]);
                    SERIAL_PRINT("-");
                }
                SERIAL_PRINTLN("END");
#endif
            }
        }
    } // ENDWHILE
}