hi

Dependencies:   CANMsg

main.cpp

Committer:
davidyu
Date:
10 months ago
Revision:
1:983b2e230859
Parent:
0:69bbc94cae9a

File content as of revision 1:983b2e230859:

#include "mbed.h"
#include "canIds.h"
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>

#define BOARD1          // Comment this for one board, BOARD1 sends

#define CHAR16                      3
#define CHAR32                      5
#define NUM_VOLTAGE_READINGS        31
#define NUM_TEMP_READINGS           62


#define MPPT_BASE_R     0x710 // MPPT Base ID (Request)
#define MPPT_BASE_A     0x770 // MPPT Base ID (Answer)
#define MPPT_OFF        0xF //MPPT Offset ID (set by DIP switch on board)
 
Serial          pc(USBTX, USBRX);
CAN             can(PA_11, PA_12);       // L432KC
// CAN             can(PD_0, PD_1);            // 429ZI
DigitalOut      led(LED1);
CANMessage      msg;
 
 
// Array Temperatures
AnalogIn Vtemp1(PA_3); // RTD temperature sensor (pin A2)
AnalogIn Vtemp2(PA_4); // RTD temperature sensor (pin A3)
double tempmeas1;
double tempmeas2;
double avgval;
double n;
double tempoff1 = -1.8; // Offset in deg C to calibrate RTD 1
double tempoff2 = -1.8; // Offset in deg C to calibrate RTD 2
double slope1 = 1.25; // Slope correction to RTD
double slope2 = 1.25; // Slope correction to RTD

int counter = 0;
#ifdef BOARD1
    int CanID = 0x0000001;
#else
    int CanID = 0x0000002;
#endif
 
// https://stackoverflow.com/questions/24420246/c-function-to-convert-float-to-byte-array
// Receives temperatures from the array
void receiveArrayTemp(){
    // Temperature Measure 1
    avgval = 0.0;
    n=0;
    while(n<100){
        tempmeas1 = Vtemp1.read()*3.3;
        avgval=avgval+tempmeas1;
        n++;
    }
    tempmeas1 = tempoff1 + slope1*(avgval/100.0)*150.0/2.92;
    
    // Temperature Measure 2
    avgval = 0.0;
    n=0;
    while(n<100){
        tempmeas2 = Vtemp2.read()*3.3;
        avgval=avgval+tempmeas2;
        n++;
    }
    tempmeas2 = tempoff2 + slope2*(avgval/100.0)*150.0/2.92;
}

void float2Bytes(float val,uint8_t* bytes_array){
    uint8_t temp;
    // Create union of shared memory space
    union {
        float float_variable;
        uint8_t temp_array[4];
    } u;
    // Overite bytes of union with float variable
    u.float_variable = val;
    // Assign bytes to input array
    memcpy(bytes_array, u.temp_array, 4);
    temp = bytes_array[3];
    bytes_array[3] = bytes_array[0];
    bytes_array[0] = temp;
    temp = bytes_array[2];
    bytes_array[2] = bytes_array[1];
    bytes_array[1] = temp;
}

float bytes2Float(uint8_t* bytes_array) {
    union {
        float f;
        uint8_t b[4];
    } u;
    u.b[3] = bytes_array[0];
    u.b[2] = bytes_array[1];
    u.b[1] = bytes_array[2];
    u.b[0] = bytes_array[3];
    return u.f;
} 
 
// TODO: use snprintf to convert int, uint16_t stuff into char array for can message
// sprintf(&voltagesChar[6*i], "%d", voltages[i]); 
void generateValues() {
    // state of charge (SOC)
    //uint32_t socData = (rand() % (100 - 0 + 1)) + 0;
    float socData = (rand() % (100000 - 0 + 1)) + 0;
    socData = socData / 1000.0;
    uint8_t socDataBytes[4];                 // TODO: make separate ones once we thread
    float2Bytes(socData, &socDataBytes[0]);
    if(can.write(CANMessage(SOC_DATA, (char*)(socDataBytes), 4))) {
        pc.printf("Sent SOC_DATA: %f\n", socData);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
//    // current data, 50-60A (while moving) fixed point, 32 bits
//    // num = (rand() % (upper - lower + 1)) + lower
    float currentData = (rand() % (60000 - 50000 + 1)) + 50000;
    currentData = currentData / 1000.0;
    uint8_t bytes[4];
    float2Bytes(currentData, &bytes[0]);
    
    if(can.write(CANMessage(CURRENT_DATA, (char*)(bytes), 4))) {
        pc.printf("Sent CURRENT_DATA: %f\n", currentData);    
    } else {
        pc.printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
    // voltage data, fixed point 2.7-4V
    // 16 bit
    float voltages[NUM_VOLTAGE_READINGS];
    for (int i = 0; i < NUM_VOLTAGE_READINGS; i++) {
        float voltageData = (rand() % (40000 - 27000 + 1)) + 27000;   
        voltageData = voltageData / 1000.0;
        //uint8_t bytes[4];
        float2Bytes(voltageData, &bytes[0]);
        
        uint8_t data[5];
        data[0] = i;
        data[1] = bytes[0];
        data[2] = bytes[1];
        data[3] = bytes[2];
        data[4] = bytes[3];
        if(can.write(CANMessage(VOLTAGE_DATA, (char*)(data), 5))) {
            pc.printf("Sent VOLTAGE_DATA: %f\n", voltageData); 
        } else {
            printf("Cannot write to CAN\n");
            NVIC_SystemReset();
        }
        wait(0.1);
    }
    
    // temperature data, 0-100C fixed point res 0.01
    // 16 bit
    float temperatures[NUM_TEMP_READINGS];
    
    for (int i = 0; i < NUM_TEMP_READINGS; i++) {
        float tempData = (rand() % (73000 - 0 + 1)) + 0;
        tempData = tempData / 1000.0;
        float2Bytes(tempData, &bytes[0]);
        
        uint8_t data[5];
        data[0] = i;
        data[1] = bytes[0];
        data[2] = bytes[1];
        data[3] = bytes[2];
        data[4] = bytes[3];
        
        if(can.write(CANMessage(TEMPERATURE_DATA, (char*)(data), 5))) {
            pc.printf("Sent TEMP_DATA: %f\n", tempData);
        } else {
            printf("Cannot write to CAN\n");
            NVIC_SystemReset();
        }
        wait(0.1);
    }
    
    // TODO: if these flags are not at the bottom of the function, it will skip values. figure this out.
    // bps trip always 0
    uint8_t bpsTrip = 0;
    if(can.write(CANMessage(BPS_TRIP, (char*)&bpsTrip, 1))) {
        pc.printf("Sent BPS_TRIP: %d\n", bpsTrip);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
    // bps all clear always 1
    uint8_t bpsClear = 1;
    if(can.write(CANMessage(BPS_ALL_CLEAR, (char*)&bpsClear, 1))) {
        pc.printf("Sent BPS_CLEAR: %d\n", bpsClear);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
    // bps off always 0
    uint8_t bpsOff = 0;
    if(can.write(CANMessage(BPS_OFF, (char*)&bpsOff, 1))) {
        pc.printf("Sent BPS_OFF: %d\n", bpsOff);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
    // watchdog triggered, 0 always
    uint8_t wdog = 0;
    if(can.write(CANMessage(WDOG_TRIGGERED, (char*)&wdog, 1))) {
        pc.printf("Sent WDOG_TRIGGERED: %d\n", wdog);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
    // TODO: can error, bps command message?
    //wait(1.0);
    // Send array temepratures
    receiveArrayTemp();
//    uint8_t arrayTemp1 = 0;
//    uint8_t arrayTemp2 = 0;
//    
//    double tempmeas1;
//    double tempmeas2;
    float2Bytes((float) tempmeas1, &bytes[0]);
    if(can.write(CANMessage(ARRAY_TEMP_1,(char*)&tempmeas1, 4))) {
        pc.printf("Sent ARRAY_TEMP_1: %.1f\n", tempmeas1);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
    
    float2Bytes((float) tempmeas2, &bytes[0]);
    if(can.write(CANMessage(ARRAY_TEMP_2,(char*)&tempmeas2, 4))) {
        pc.printf("Sent ARRAY_TEMP_2: %.1f\n", tempmeas2);    
    } else {
        printf("Cannot write to CAN\n");
        NVIC_SystemReset();
    }
} 

float receiveCan(void) {
    uint32_t received;
    /*
    int cansuccess = 0;
    float data[2];
    data[1] = 0.0;
    int id = MPPT_BASE_R + MPPT_OFF;
 
    if (!can.write(CANMessage(id, (char*)data, 8)))
        printf("Request to MPPT failed \n\r");
    */
    if (can.read(msg)) {
        received = *((uint32_t*) msg.data);
    } else {
        return -1;
    } 
    pc.printf("-----------------------\r\n");
    
    /*
    if(msg.id == (MPPT_BASE_A+MPPT_OFF) ) {
        
        AnalogIn Vtemp1(PA_3); // RTD temperature sensor (pin A2)
AnalogIn Vtemp2(PA_4); // RTD temperature sensor (pin A3)
        
        double tempmeas1;
    double tempmeas2;
    double tempoff1 = -1.8; // Offset in deg C to calibrate RTD 1
    double tempoff2 = -1.8; // Offset in deg C to calibrate RTD 2
    double slope1 = 1.25; // Slope correction to RTD
    double slope2 = 1.25; // Slope correction to RTD
    double avgval;
    int n = 0;
    
    float vscale = 150.49; // Input voltage scale factor, mV/LSB (Drivetek datasheet)
    float vscaleout = 208.79; // Output voltage scale factor, mV/LSB (Drivetek datasheet)
    float iscale = 8.72; // Input current scale factor, mA/LSB (Drivetek datasheet)
    float mpptpwr;
   
    cansuccess = 1;
    int involtmsb = msg.data[0] & 0x3; // Bit 0 and Bit 1 of Byte 1 are MSB Uin
    int involtlsb = msg.data[1]; // Byte 2 is LSB of Uin
    int involt = (involtmsb<<8)|(involtlsb);    
    int outvoltmsb = msg.data[4] & 0x3; // Bit 0 and Bit 1 of Byte 5 are MSB Uout
    int outvoltlsb = msg.data[5]; // Byte 6 is LSB of Uout
    int outvolt = (outvoltmsb<<8)|(outvoltlsb);    
    int incurmsb = msg.data[2] & 0x3; // Bit 0 and Bit 1 of Byte 3 are MSB Uin
    int incurlsb = msg.data[3]; // Byte 4 is LSB of Iin
    int incur = (incurmsb<<8)|(incurlsb);
    mpptpwr = (involt*vscale/1000.0)*(incur*iscale/1000.0);
   
    avgval = 0.0;
    n=0;
    while(n<100){
        tempmeas1 = Vtemp1.read()*3.3;
        avgval=avgval+tempmeas1;
        n++;
        }
    tempmeas1 = tempoff1 + slope1*(avgval/100.0)*150.0/2.92;
    avgval = 0.0;
    n=0;
    while(n<100){
        tempmeas2 = Vtemp2.read()*3.3;
        avgval=avgval+tempmeas2;
        n++;
        }
    tempmeas2 = tempoff2 + slope2*(avgval/100.0)*150.0/2.92;
    printf("Array Temperature 1 = %.1f", tempmeas1);
    printf(" deg C   ");
    printf("Array Temperature 2 = %.1f", tempmeas2);
    printf(" deg C \r\n");
    printf("MPPT In Vol = %.1f", involt*vscale/1000.0);
    printf("V Out Vol = %.1f", outvolt*vscaleout/1000.0);
    printf("V Input Cur = %.1f", incur*iscale/1000.0);
    printf("A In Pwr = %.2f", mpptpwr);
    printf("W \r\n");
    }
    */
    if (msg.id == DC_BUS_CURRENT){
        float DCbuscur = bytes2Float(msg.data);
        pc.printf("DC bus current is %f\n", DCbuscur);
        return DCbuscur;
    } else if (msg.id == DC_BUS_VOLTAGE){
        float DCbusvolt = bytes2Float(msg.data);
        pc.printf("DC bus voltage is %f\n", DCbusvolt);
        return DCbusvolt;
    } else if (msg.id == PHASE_B_CURRENT){
        float phaseBcurrent = bytes2Float(msg.data);
        pc.printf("Phase B current is %f\n", phaseBcurrent);
        return phaseBcurrent;
    } else if (msg.id == PHASE_C_CURRENT){
        float phaseCcurrent = bytes2Float(msg.data);
        pc.printf("Phase C current is %f\n", phaseCcurrent);
        return phaseCcurrent;
    } else if (msg.id == VEHICLE_VELOCITY){
        float vehicleVel = bytes2Float(msg.data);
        pc.printf("Vehicle velocity is %f\n", vehicleVel);
        return vehicleVel;
    } else if (msg.id == MOTOR_VELOCITY){
        float motorVel = bytes2Float(msg.data);
        pc.printf("Motor velocity is %f\n", motorVel);
        return motorVel;
    } else if (msg.id == VD){
        float vd = bytes2Float(msg.data);
        pc.printf("Velocity vector D is %f\n", vd);
        return vd;
    } else if (msg.id == VQ){
        float vq = bytes2Float(msg.data);
        pc.printf("Velocity vector Q is %f\n", vq);
        return vq;
    }  else if (msg.id == ID){
        float Id = bytes2Float(msg.data);
        pc.printf("Current vector D is %f\n", Id);
        return Id;
    } else if (msg.id == IQ){
        float Iq = bytes2Float(msg.data);
        pc.printf("Current vector Q is %f\n", Iq);
        return Iq;
    } else if (msg.id == BEMFD){
        float BEMFd = bytes2Float(msg.data);
        pc.printf("BackEMF Measurement D is %f\n", BEMFd);
        return BEMFd;
    } else if (msg.id == BEMFQ){
        float BEMFq = bytes2Float(msg.data);
        pc.printf("BackEMF Measurement Q is %f\n", BEMFq);
        return BEMFq;
    } else if (msg.id == HEAT_SINK_TEMPERATURE){
        float heatSinkTemp = bytes2Float(msg.data);
        pc.printf("Heat sink temperature is %f\n", heatSinkTemp);
        return heatSinkTemp;
    } else if (msg.id == MOTOR_TEMPERATURE){
        float motorTemp = bytes2Float(msg.data);
        pc.printf("Motor temperature is %f\n", motorTemp);
        return motorTemp;
    } else if (msg.id == DC_BUS_AMP_HOURS){
        float DCBusAmpHours = bytes2Float(msg.data);
        pc.printf("DC Bus AmpHours is %f\n", DCBusAmpHours);
        return DCBusAmpHours;
    } else if (msg.id == ODOMETER){
        float odometerValue = bytes2Float(msg.data);
        pc.printf("Odomoter value is %f\n", odometerValue);
        return odometerValue;
    } 
    
    // BPS
    if (msg.id == BPS_TRIP) {
        received = msg.data[0];
        pc.printf("BPS Trip is %d\n", received);
    } else if (msg.id == BPS_ALL_CLEAR) {
        received = msg.data[0];
        pc.printf("BPS All Clear is %d\n", received);
    } else if (msg.id == BPS_OFF) {
        received = msg.data[0];
        // sd card stuff
        pc.printf("BPS Off is %d\n", received);    
    } else if (msg.id == WDOG_TRIGGERED) {
        received = msg.data[0];
        pc.printf("WDOG Triggered is %d\n", received);    
    } else if (msg.id == CAN_ERROR) {
        received = msg.data[0];
        pc.printf("CAN Error is %d\n", received);    
    } else if (msg.id == VOLTAGE_DATA) {
        // voltage
        float voltage = bytes2Float(&msg.data[1]);
        pc.printf("Voltage is %.3fV from array index %d\n", voltage, msg.data[0]);
        return voltage;
    } else if (msg.id == TEMPERATURE_DATA){
        // temperature   
        float temp = bytes2Float(&msg.data[1]);
        pc.printf("Temp is %.3f from array index %d\n", temp, msg.data[0]);  
        return temp;
    } else if(msg.id == SOC_DATA){
        // state of charge
        float soc = bytes2Float(msg.data);
        pc.printf("SoC is %.3f%%\n", soc);
        return received;
    } else if(msg.id == CURRENT_DATA){
        // current
        float current = bytes2Float(msg.data);
        pc.printf("Current is %.3fA\n", current); 
        return current;
    }
    
    // Motor controller
    /*
    else if (msg.id == DC_BUS_CURRENT){
        float DCbuscur = bytes2Float(msg.data);
        pc.printf("DC bus current is %f\n", DCbuscur);
        return DCbuscur;
    } else if (msg.id == DC_BUS_VOLTAGE){
        float DCbusvolt = bytes2Float(msg.data);
        pc.printf("DC bus voltage is %f\n", DCbusvolt);
        return DCbusvolt;
    } else if (msg.id == PHASE_B_CURRENT){
        float phaseBcurrent = bytes2Float(msg.data);
        pc.printf("Phase B current is %f\n", phaseBcurrent);
        return phaseBcurrent;
    } else if (msg.id == PHASE_C_CURRENT){
        float phaseCcurrent = bytes2Float(msg.data);
        pc.printf("Phase C current is %f\n", phaseCcurrent);
        return phaseCcurrent;
    } else if (msg.id == VEHICLE_VELOCITY){
        float vehicleVel = bytes2Float(msg.data);
        pc.printf("Vehicle velocity is %f\n", vehicleVel);
        return vehicleVel;
    } else if (msg.id == MOTOR_VELOCITY){
        float motorVel = bytes2Float(msg.data);
        pc.printf("Motor velocity is %f\n", motorVel);
        return motorVel;
    } else if (msg.id == VD){
        float vd = bytes2Float(msg.data);
        pc.printf("Velocity vector D is %f\n", vd);
        return vd;
    } else if (msg.id == VQ){
        float vq = bytes2Float(msg.data);
        pc.printf("Velocity vector Q is %f\n", vq);
        return vq;
    }  else if (msg.id == ID){
        float Id = bytes2Float(msg.data);
        pc.printf("Current vector D is %f\n", Id);
        return Id;
    } else if (msg.id == IQ){
        float Iq = bytes2Float(msg.data);
        pc.printf("Current vector Q is %f\n", Iq);
        return Iq;
    } else if (msg.id == BEMFD){
        float BEMFd = bytes2Float(msg.data);
        pc.printf("BackEMF Measurement D is %f\n", BEMFd);
        return BEMFd;
    } else if (msg.id == BEMFQ){
        float BEMFq = bytes2Float(msg.data);
        pc.printf("BackEMF Measurement Q is %f\n", BEMFq);
        return BEMFq;
    } else if (msg.id == HEAT_SINK_TEMPERATURE){
        float heatSinkTemp = bytes2Float(msg.data);
        pc.printf("Heat sink temperature is %f\n", heatSinkTemp);
        return heatSinkTemp;
    } else if (msg.id == MOTOR_TEMPERATURE){
        float motorTemp = bytes2Float(msg.data);
        pc.printf("Motor temperature is %f\n", motorTemp);
        return motorTemp;
    } else if (msg.id == DC_BUS_AMP_HOURS){
        float DCBusAmpHours = bytes2Float(msg.data);
        pc.printf("DC Bus AmpHours is %f\n", DCBusAmpHours);
        return DCBusAmpHours;
    } else if (msg.id == ODOMETER){
        float odometerValue = bytes2Float(msg.data);
        pc.printf("Odomoter value is %f\n", odometerValue);
        return odometerValue;
    } 
    */
    
    
    pc.printf("-----------------------\r\n");
    //wait(0.5);
    return -1;
} 

int main() {
    pc.baud(9600);          // set serial speed
    can.frequency(125000); // max 1Mbps, usually 150000
    pc.printf("-----------------\r\n");
    pc.printf("    main loop    \r\n");
    pc.printf("this is board %d \r\n", CanID);
    pc.printf("-----------------\r\n");
    CANMessage msg;
    led = 0;
    
    while(1) {
       // Write CAN, comment for one board
       #ifdef BOARD1
        generateValues();
       #else
       // Read CAN, comment for the other
        receiveCan();
       #endif
       
        // comment for push
         // wait(1.0);
       
    }
}