Diff: main.cpp

Revision:

2:2a4822c7c91a

Parent:

1:863bdd011cf8

Child:

3:f282664610ba

--- a/main.cpp	Sat Nov 09 17:33:29 2019 +0000
+++ b/main.cpp	Sat Nov 09 18:35:01 2019 +0000
@@ -1,10 +1,6 @@
-// Motor Control Board Program
-// This program operates the Tritium controller and also sends data
-// over the separate car CAN.
+#include "mbed.h"
+#include "shared_values.h"
 
-#include "mbed.h"
-
-// CAN base address and offsets (Tritium)
 #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
@@ -23,133 +19,101 @@
 #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
+#define DC_BUS_CURRENT 0x600
+#define DC_BUS_VOLTAGE 0x601
+#define PHASE_B_CURRENT 0x602
+#define PHASE_C_CURRENT 0x603
+#define VEHICLE_VELOCITY 0x604
+#define MOTOR_VELOCITY 0x605
+#define VD 0x606
+#define VQ 0x607
+#define ID 0x608
+#define IQ 0x609
+#define BEMFD 0x60A
+#define BEMFQ 0x60B
+#define HEAT_SINK_TEMPERATURE 0x60C
+#define MOTOR_TEMPERATURE 0x60D
+#define DC_BUS_AMP_HOURS 0x60E
+#define ODOMETER 0x60F
 
-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)
+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);
 
-AnalogIn ain(PB_0);
-
-DigitalOut LED8(PF_2);
-DigitalOut LED7(PA_7);
-DigitalOut LED6(PF_10);
-DigitalOut LED5(PF_5);
-DigitalOut LED4a(PF_3);
-DigitalOut LED3a(PC_3);
-DigitalOut LED2a(PC_0);
-DigitalOut LED1a(PA_3);
-
-#define MAX_VELOCITY 100 // motor velocity in m/s
-#define MAX_CURRENT 1.0  // desired motor current as percentage of max current
-
-int main()
+// https://stackoverflow.com/questions/24420246/c-function-to-convert-float-to-byte-array
+void float2Bytes(float val, uint8_t *bytes_array)
 {
-    float current = MAX_CURRENT;
-    float velocity = MAX_VELOCITY;
-    float bus_current = MAX_CURRENT;
-    float DCbuscur;
-    float DCbusvolt;
-    double pedal_position;
-    float data[2];
-    float data2[2];
-    float meas;
-    double avgval;
-    int n;
-    int dummy;
-    int alive;
+    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;
+}
 
-    // other ids we need to read
-    float phaseBcurrent;
-    float phaseCcurrent;
-    float vehicleVel;
-    float motorVel;
-    float vd;
-    float vq;
-    float Id;
-    float Iq;
-    float BEMFd;
-    float BEMFq;
-    float heatSinkTemp;
-    float motorTemp;
-    float DCBusAmpHours;
-    float odometerValue;
+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;
+}
 
-    //char const * serial = "0002173";
-    // can1.frequency(500000);
-    int id;
-    int id2;
-    int id3;
-    char rdata[8];
-    CANMessage msg;
-    dummy = 0;
-    alive = 0;
+union {
+    char rcvdata[4];
+    float rxdata;
+} urxdata;
 
-    union {
-        char rcvdata[4];
-        float rxdata;
-    } urxdata;
+CANMessage msg;
+char rdata[8];
+char rdata2[8];
 
-    id = DC_BASE + DC_DRIVE;
-    id2 = DC_BASE + DC_POWER;
-    id3 = MC_BASE + DC_POWER;
-
+void pedal()
+{
     while (1)
     {
         n = 0;
         avgval = 0.0;
         while (n < 100)
         {
-            meas = ain.read();
+            meas = poop.read();
             avgval = avgval + meas;
             n++;
         }
         pedal_position = avgval / 100.0;
 
-        if (pedal_position > 0.005)
-            LED1a = 1;
-        else
-            LED1a = 0;
-        if (pedal_position > 0.01)
-            LED2a = 1;
-        else
-            LED2a = 0;
-        if (pedal_position > 0.015)
-            LED3a = 1;
-        else
-            LED3a = 0;
-        if (pedal_position > 0.02)
-            LED4a = 1;
-        else
-            LED4a = 0;
-        if (pedal_position > 0.025)
-            LED5 = 1;
-        else
-            LED5 = 0;
-        if (pedal_position > 0.03)
-            LED6 = 1;
-        else
-            LED6 = 0;
-        if (pedal_position > 0.035)
-            LED7 = 1;
-        else
-            LED7 = 0;
-
         current = MAX_CURRENT * pedal_position;
         velocity = 9.0;
 
@@ -165,9 +129,18 @@
             dummy = 0;
 
         wait_ms(10); // Need message every 250ms to maintain operation
+    }
+}
 
-        if (can2.read(msg) && msg.id == id3)
-        { // Tritium Bus
+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_VEL))
+        {
             for (int i = 0; i < msg.len; i++)
             {
                 rdata[i] = msg.data[i];
@@ -176,39 +149,34 @@
             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;
-            wait_ms(10); // wait to reset
-        }
-
-        // reading vehicle and motor velocity
-        else if (can2.read(msg) && msg.id == (MC_BASE + MC_VEL))
-        {
-            for (int i = 0; i < msg.len; i++)
-            {
-                rdata[i] = msg.data[i];
-            }
-            // sending value to CAR can
-            if (!can1.write(CANMessage(0x243, (char *)rdata, 8)))
-            {
-                pc.printf("Cannot write to CAN\n");
-            }
-            wait(0.1);
-            urxdata.rcvdata[3] = rdata[7];
-            urxdata.rcvdata[2] = rdata[6];
-            urxdata.rcvdata[1] = rdata[5];
-            urxdata.rcvdata[0] = rdata[4];
+            //DCbuscur = urxdata.rxdata;
             vehicleVel = 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;
             motorVel = urxdata.rxdata;
-            wait_ms(10); // wait to reset
+        }
+        else if (can2.read(msg) && msg.id == (MC_BASE + MC_BUS))
+        {
+            for (int i = 0; i < msg.len; i++)
+            {
+                rdata2[i] = msg.data[i];
+            }
+            
+            urxdata.rcvdata[3] = rdata2[7];
+            urxdata.rcvdata[2] = rdata2[6];
+            urxdata.rcvdata[1] = rdata2[5];
+            urxdata.rcvdata[0] = rdata2[4];
+            //vehicleVel = urxdata.rxdata;
+            DCbuscur = urxdata.rxdata;
+            urxdata.rcvdata[3] = rdata2[3];
+            urxdata.rcvdata[2] = rdata2[2];
+            urxdata.rcvdata[1] = rdata2[1];
+            urxdata.rcvdata[0] = rdata2[0];
+            //motorVel = urxdata.rxdata;
+            DCbusvolt = urxdata.rxdata;
         }
 
         // reading phase currents
@@ -228,7 +196,6 @@
             urxdata.rcvdata[1] = rdata[1];
             urxdata.rcvdata[0] = rdata[0];
             phaseBcurrent = urxdata.rxdata;
-            wait_ms(10); // wait to reset
         }
 
         // reading motor voltage vector
@@ -267,7 +234,6 @@
             urxdata.rcvdata[1] = rdata[1];
             urxdata.rcvdata[0] = rdata[0];
             Iq = urxdata.rxdata;
-            wait_ms(10); // wait to reset
         }
 
         // reading back emf
@@ -287,7 +253,6 @@
             urxdata.rcvdata[1] = rdata[1];
             urxdata.rcvdata[0] = rdata[0];
             BEMFq = urxdata.rxdata;
-            wait_ms(10); // wait to reset
         }
 
         // reading heatsink and motor temp
@@ -307,7 +272,6 @@
             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
@@ -327,14 +291,15 @@
             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 \n\r");
-            printf(" Requested Motor Current: %f\n\r", current);
-            printf(" Requested Motor Velocity: %f\n\r", velocity);
+
+            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);
@@ -370,12 +335,220 @@
             printf("\r\n");
             printf(" Odometer (Distance) (m) = %f", odometerValue);
             printf("\r\n");
+        }
+    }
+}
 
-            // blinking LED
-            LED8 = !LED8;
-            if (!can1.write(CANMessage(id, (char *)data, 8))) //send current and velocity over car CAN
-                printf("Car CAN failed \n\r");
+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)");
         }
-        alive++;
+        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");
+        }
     }
-}
\ No newline at end of file
+}
+
+int main()
+{
+    Thread recc(receiveCAN);
+    wait(1);
+    Thread Indicators(pedal);
+    wait(2);
+    Thread send(sendCAN);
+
+    while (1)
+    {
+        //pc.printf("main\r\n");
+        /*
+        uint8_t bytes1[4];
+        float foo = 42.0;
+        //float2Bytes(foo, &bytes1[0]);
+        if (can1.write(CANMessage(MC_BASE + MC_VEL, (char*)rdata2, 8))) {
+            pc.printf("Sent Velocity = %f", foo);
+        }
+        else {
+            pc.printf("Cannot write to CAN\n");
+        }
+        wait(0.1);
+        */
+    }
+}