Diff: main.cpp

Revision:

0:c61d18b01f8c

Child:

1:863bdd011cf8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 09 15:41:47 2019 +0000
@@ -0,0 +1,518 @@
+#include "mbed.h"
+#include "shared_values.h"
+
+#define DC_BASE 0x220  // Driver controls base address
+#define DC_DRIVE 0x01  // Offset for motor drive command
+#define DC_POWER 0x02  // Offset for motor power command
+#define DC_RESET 0x03  // Offset for reset command
+#define DC_SWITCH 0x04 // Offset for phase current measurement
+
+#define MC_BASE 0x240 // Motor controls base address
+#define MC_BUS 0x02   // Bus measurement offset
+#define MC_VEL 0x03   // Velocity measurement offset
+#define MC_PHCUR 0x04 // Phase Current offset
+#define MC_VOVEC 0x05 // Voltage Vector offset
+#define MC_CUVEC 0x06 // current vector offset
+#define MC_BEMF 0x07  // back emf offset
+#define MC_TEMP 0x0B  // heat sink and motor temp offset
+#define MC_AMPH 0x0E  // odometer and bus amp ohours measuremeant
+#define MAX_VELOCITY 100 // motor velocity in m/s
+#define MAX_CURRENT 1.0  // desired motor current as percentage of max current
+
+#define DC_BUS_CURRENT 0x900
+#define DC_BUS_VOLTAGE 0x901
+#define PHASE_B_CURRENT 0x902
+#define PHASE_C_CURRENT 0x903
+#define VEHICLE_VELOCITY 0x904
+#define MOTOR_VELOCITY 0x905
+#define VD 0x906
+#define VQ 0x907
+#define ID 0x908
+#define IQ 0x909
+#define BEMFD 0x90A
+#define BEMFQ 0x90B
+#define HEAT_SINK_TEMPERATURE 0x90C
+#define MOTOR_TEMPERATURE 0x90D
+#define DC_BUS_AMP_HOURS 0x90E
+#define ODOMETER 0x90F
+
+float current = MAX_CURRENT;
+float velocity = MAX_VELOCITY;
+float bus_current = MAX_CURRENT;
+double pedal_position;
+double avgval;
+float data[2];
+float data2[2];
+float meas = 0;
+int n;
+int dummy;
+int alive;
+
+int id = DC_BASE + DC_DRIVE;
+int id2 = DC_BASE + DC_POWER;
+int id3 = MC_BASE + DC_POWER;
+
+CAN can1(PD_0, PD_1 /*, 125000*/); // can1 is car CAN  (Rx, Tx, speed)
+CAN can2(PB_5, PB_6 /*, 50000*/);  // can2 is motor controller CAN  (Rx, Tx, speed)
+AnalogIn poop(PB_0);
+Serial pc(USBTX, USBRX);
+
+// https://stackoverflow.com/questions/24420246/c-function-to-convert-float-to-byte-array
+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;
+}
+
+union {
+    char rcvdata[4];
+    float rxdata;
+} urxdata;
+
+CANMessage msg;
+char rdata[8];
+
+
+void pedal()
+{
+    while (1)
+    {
+        n = 0;
+        avgval = 0.0;
+        while (n < 100)
+        {
+            meas = poop.read();
+            avgval = avgval + meas;
+            n++;
+        }
+        pedal_position = avgval / 100.0;
+
+        current = MAX_CURRENT * pedal_position;
+        velocity = 9.0;
+
+        data[1] = current; // Flipped because of endianness
+        data[0] = velocity;
+
+        if (!can2.write(CANMessage(id, (char *)data, 8))) // send current and velocity to Tritum
+            printf("Drive failed \n\r");
+
+        data2[1] = bus_current;
+        data2[0] = 0.0;
+        if (!can2.write(CANMessage(id2, (char *)data2, 8)))
+            dummy = 0;
+
+        wait_ms(10); // Need message every 250ms to maintain operation
+    }
+}
+
+
+void receiveCAN()
+{
+    can1.frequency(125000);
+    can2.frequency(50000);
+    while (1)
+    {
+        pc.printf("inside thread \r\n");
+        if (can2.read(msg) && msg.id == (MC_BASE + MC_BUS))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            DCbuscur = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            DCbusvolt = urxdata.rxdata;
+        }
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_VEL))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            vehicleVel = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            motorVel = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        // reading phase currents
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_PHCUR))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            phaseCcurrent = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            phaseBcurrent = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        // reading motor voltage vector
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_VOVEC))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            vd = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            vq = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        // reading current vector
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_CUVEC))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            Id = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            Iq = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        // reading back emf
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_BEMF))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            BEMFd = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            BEMFq = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        // reading heatsink and motor temp
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_TEMP))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            heatSinkTemp = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            motorTemp = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        // reading odometer and bus amp ohours measuremeant
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_AMPH))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata[i] = msg.data[i];
+            }
+            urxdata.rcvdata[3] = rdata[7];
+            urxdata.rcvdata[2] = rdata[6];
+            urxdata.rcvdata[1] = rdata[5];
+            urxdata.rcvdata[0] = rdata[4];
+            DCBusAmpHours = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata[3];
+            urxdata.rcvdata[2] = rdata[2];
+            urxdata.rcvdata[1] = rdata[1];
+            urxdata.rcvdata[0] = rdata[0];
+            odometerValue = urxdata.rxdata;
+            wait_ms(10); // wait to reset
+        }
+
+        if (alive % 100 == 0)
+        {
+
+            printf("Motor board is running");
+            printf("\r\n");
+            //printf(" Requested Motor Current: %f\n\r", current);
+            //printf(" Requested Motor Velocity: %f\n\r", velocity);
+            printf(" DC Bus Current (A) = %f", DCbuscur);
+            printf("\r\n");
+            printf(" DC Bus Voltage (V) = %f", DCbusvolt);
+            printf("\r\n");
+
+            // Printing other values
+            printf(" Vehicle Velocity (RPM) = %f", vehicleVel);
+            printf("\r\n");
+            printf(" Motor Velocity (V) = %f", motorVel);
+            printf("\r\n");
+            printf(" Phase B Current (A-rms) = %f", phaseBcurrent);
+            printf("\r\n");
+            printf(" Phase C Current (A-rms) = %f", phaseCcurrent);
+            printf("\r\n");
+            printf(" Vd (V) = %f", vd);
+            printf("\r\n");
+            printf(" Vq (V) = %f", vq);
+            printf("\r\n");
+
+            printf(" Id (A) = %f", Id);
+            printf("\r\n");
+            printf(" Iq (A) = %f", Iq);
+            printf("\r\n");
+            printf(" BEMFd (V) = %f", BEMFd);
+            printf("\r\n");
+            printf(" BEMFq (V) = %f", BEMFq);
+            printf("\r\n");
+            printf(" Heat Sink Temperature (Celsius) = %f", heatSinkTemp);
+            printf("\r\n");
+            printf(" Motor Temperature (Celsius) = %f", motorTemp);
+            printf("\r\n");
+            printf(" DC Bus (Ah) = %f", DCBusAmpHours);
+            printf("\r\n");
+            printf(" Odometer (Distance) (m) = %f", odometerValue);
+            printf("\r\n");
+        }
+    }
+}
+
+int counter = 0;
+int CAN_FLAG = 0;
+
+void sendCAN()
+{
+    while (1)
+    {
+        uint8_t bytes1[4];
+        float2Bytes(DCbuscur, &bytes1[0]);
+        if (can1.write(CANMessage(DC_BUS_CURRENT, (char*)(bytes1), 4))) {
+            pc.printf("Sent DC Bus Current");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes2[4];
+        float2Bytes(DCbusvolt, &bytes2[0]);
+        if (can1.write(CANMessage(DC_BUS_VOLTAGE, (char*)(bytes2), 4))) {
+            pc.printf("Sent DC Bus Voltage");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes3[4];
+        float2Bytes(vehicleVel, &bytes3[0]);
+        if (can1.write(CANMessage(VEHICLE_VELOCITY, (char*)(bytes3), 4))) {
+            pc.printf("Sent Vehicle Velocity (RPM)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes4[4];
+        float2Bytes(motorVel, &bytes4[0]);
+        if (can1.write(CANMessage(MOTOR_VELOCITY, (char*)(bytes4), 4))) {
+            pc.printf("Sent Motor Velocity (V)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes5[4];
+        float2Bytes(phaseBcurrent, &bytes5[0]);
+        if (can1.write(CANMessage(PHASE_B_CURRENT, (char*)(bytes5), 4))) {
+            pc.printf("Sent Phase B Current");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes6[4];
+        float2Bytes(phaseCcurrent, &bytes6[0]);
+        if (can1.write(CANMessage(PHASE_C_CURRENT, (char*)(bytes6), 4))) {
+            pc.printf("Sent Phase C Current");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes7[4];
+        float2Bytes(vd, &bytes7[0]);
+        if (can1.write(CANMessage(VD, (char*)(bytes7), 4))) {
+            pc.printf("Sent Vd (V)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes8[4];
+        float2Bytes(vq, &bytes8[0]);
+        if (can1.write(CANMessage(VQ, (char*)(bytes8), 4))) {
+            pc.printf("Sent Vq (V)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes9[4];
+        float2Bytes(Id, &bytes9[0]);
+        if (can1.write(CANMessage(ID, (char*)(bytes9), 4))) {
+            pc.printf("Sent Id (A)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes10[4];
+        float2Bytes(Iq, &bytes10[0]);
+        if (can1.write(CANMessage(IQ, (char*)(bytes10), 4))) {
+            pc.printf("Sent Iq (A)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes11[4];
+        float2Bytes(BEMFd, &bytes11[0]);
+        if (can1.write(CANMessage(BEMFD, (char*)(bytes11), 4))) {
+            pc.printf("Sent BEMFd");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes12[4];
+        float2Bytes(BEMFq, &bytes12[0]);
+        if (can1.write(CANMessage(BEMFQ, (char*)(bytes12), 4))) {
+            pc.printf("Sent BEMFq");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes13[4];
+        float2Bytes(heatSinkTemp, &bytes13[0]);
+        if (can1.write(CANMessage(HEAT_SINK_TEMPERATURE, (char*)(bytes13), 4))) {
+            pc.printf("Sent Heat Sink Temperature (Celsius)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes14[4];
+        float2Bytes(motorTemp, &bytes14[0]);
+        if (can1.write(CANMessage(MOTOR_TEMPERATURE, (char*)(bytes14), 4))) {
+            pc.printf("Sent Motor Temperature (Celsius)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes15[4];
+        float2Bytes(DCBusAmpHours, &bytes15[0]);
+        if (can1.write(CANMessage(DC_BUS_AMP_HOURS, (char*)(bytes15), 4))) {
+            pc.printf("Sent DC Bus (Ah)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+
+        uint8_t bytes16[4];
+        float2Bytes(odometerValue, &bytes16[0]);
+        if (can1.write(CANMessage(ODOMETER, (char*)(bytes16), 4))) {
+            pc.printf("Sent Odometer (Distance) (m)");
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+    }
+}
+
+
+int main()
+{
+    Thread recc(receiveCAN);
+    wait(1);
+    Thread Indicators(pedal);
+    wait(2);
+   // Thread send(sendCAN);
+
+    while (1)
+    {
+        uint8_t bytes1[4];
+        float foo = 42.0;
+        float2Bytes(foo, &bytes1[0]);
+        if (can1.write(CANMessage(DC_BUS_CURRENT, (char*)(bytes1), 4))) {
+            pc.printf("Sent DC Bus Current = %f", foo);
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+        wait(0.1);
+    }
+}