Diff: main.cpp

Revision:

4:7d027ceba0a0

Parent:

3:f282664610ba

Child:

5:f55622eacb87

--- a/main.cpp	Sat Nov 09 18:50:12 2019 +0000
+++ b/main.cpp	Sat Nov 09 20:30:32 2019 +0000
@@ -1,16 +1,16 @@
 // Motor Control Board Program
 // This program operates the Tritium controller and also sends data
 // over the separate car CAN.
-
+ 
 #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
 #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
@@ -22,7 +22,7 @@
 #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
@@ -39,13 +39,13 @@
 #define MOTOR_TEMPERATURE 0x90D
 #define DC_BUS_AMP_HOURS 0x90E
 #define ODOMETER 0x90F
-
+ 
 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)
 Serial pc(USBTX, USBRX);
-
+ 
 AnalogIn ain(PB_0);
-
+ 
 DigitalOut LED8(PF_2);
 DigitalOut LED7(PA_7);
 DigitalOut LED6(PF_10);
@@ -54,10 +54,10 @@
 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() {
   float current = MAX_CURRENT;
   float velocity = MAX_VELOCITY;
@@ -72,7 +72,7 @@
   int n;
   int dummy;
   int alive;
-
+ 
   // other ids we need to read
   float phaseBcurrent;
   float phaseCcurrent;
@@ -88,7 +88,7 @@
   float motorTemp;
   float DCBusAmpHours;
   float odometerValue;
-
+ 
   //char const * serial = "0002173";
   // can1.frequency(500000);
   int id;
@@ -103,20 +103,20 @@
   char rdata7[8];
   char rdata8[8];
   char rdata9[8];
-
+ 
   CANMessage msg;
   dummy = 0;
   alive = 0;
-
+ 
     union {
       char rcvdata[4];
       float rxdata;
     } urxdata;
-
+ 
   id = DC_BASE + DC_DRIVE;
   id2 = DC_BASE + DC_POWER;
   id3 = MC_BASE + DC_POWER;
-
+ 
   while (1) {
     n = 0;
     avgval = 0.0;
@@ -126,7 +126,7 @@
         n++ ;
     }
     pedal_position = avgval/100.0;
-
+ 
     if(pedal_position > 0.005)
         LED1a = 1;
     else
@@ -155,35 +155,61 @@
         LED7 = 1;
     else
         LED7 = 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
-
+ 
    // WE ARE READING STUFFF HERE //
-    if(can2.read(msg) && msg.id == id3 ) { // Tritium Bus
+//   if(can2.read(msg) && msg.id == (MC_BASE+MC_VEL)) {
+//        for (int i = 0; i < msg.len; i++) {
+//          rdata2[i] = msg.data[i];
+//        }
+// 
+//        // sending value to CAR can
+//        if (!can1.write(CANMessage(0x243, (char*) rdata2, 8))) {
+//          pc.printf("Cannot write to CAN\n");
+//        }
+//        wait(0.01);
+// 
+//        urxdata.rcvdata[3] = rdata2[7];
+//        urxdata.rcvdata[2] = rdata2[6];
+//        urxdata.rcvdata[1] = rdata2[5];
+//        urxdata.rcvdata[0] = rdata2[4];
+//        vehicleVel = 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;
+//    }
+//       
+//   }
+   
+    if(can2.read(msg)){
+     if(msg.id == id3 ) { // Tritium Bus
         for(int i = 0; i < msg.len; i++) {
           rdata[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x242, (char*) rdata, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata[7];
         urxdata.rcvdata[2] = rdata[6];
         urxdata.rcvdata[1] = rdata[5];
@@ -195,20 +221,20 @@
         urxdata.rcvdata[0] = rdata[0];
         DCbusvolt = urxdata.rxdata;
         }
-
-
+ 
+ 
         // reading vehicle and motor velocity
-    else if(can2.read(msg) && msg.id == (MC_BASE+MC_VEL)) {
+    else if(msg.id == (MC_BASE+MC_VEL)) {
         for (int i = 0; i < msg.len; i++) {
           rdata2[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x243, (char*) rdata2, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata2[7];
         urxdata.rcvdata[2] = rdata2[6];
         urxdata.rcvdata[1] = rdata2[5];
@@ -220,19 +246,19 @@
         urxdata.rcvdata[0] = rdata2[0];
         motorVel = urxdata.rxdata;
     }
-
+ 
         // reading phase currents
-    else if(can2.read(msg) && msg.id == (MC_BASE+MC_PHCUR)) {
+    else if(msg.id == (MC_BASE+MC_PHCUR)) {
         for (int i = 0; i < msg.len; i++) {
           rdata3[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x244, (char*) rdata3, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata3[7];
         urxdata.rcvdata[2] = rdata3[6];
         urxdata.rcvdata[1] = rdata3[5];
@@ -244,19 +270,19 @@
         urxdata.rcvdata[0] = rdata3[0];
         phaseBcurrent = urxdata.rxdata;
         }
-
+ 
         // reading motor voltage vector
-    else if(can2.read(msg) && msg.id == (MC_BASE+MC_VOVEC)) {
+    else if(msg.id == (MC_BASE+MC_VOVEC)) {
         for (int i = 0; i < msg.len; i++) {
           rdata4[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x245, (char*) rdata4, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata4[7];
         urxdata.rcvdata[2] = rdata4[6];
         urxdata.rcvdata[1] = rdata4[5];
@@ -268,19 +294,19 @@
         urxdata.rcvdata[0] = rdata4[0];
         vq = urxdata.rxdata;
     }
-
+ 
         // reading current vector
-    else if(can2.read(msg) && msg.id == (MC_BASE+MC_CUVEC)) {
+    else if(msg.id == (MC_BASE+MC_CUVEC)) {
         for (int i = 0; i < msg.len; i++) {
           rdata5[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x246, (char*) rdata5, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata5[7];
         urxdata.rcvdata[2] = rdata5[6];
         urxdata.rcvdata[1] = rdata5[5];
@@ -292,19 +318,19 @@
         urxdata.rcvdata[0] = rdata5[0];
         Iq = urxdata.rxdata;
     }
-
+ 
     // reading back emf
-    else if(can2.read(msg) && msg.id == (MC_BASE+MC_BEMF)) {
+    else if(msg.id == (MC_BASE+MC_BEMF)) {
         for (int i = 0; i < msg.len; i++) {
           rdata6[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x247, (char*) rdata6, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata6[7];
         urxdata.rcvdata[2] = rdata6[6];
         urxdata.rcvdata[1] = rdata6[5];
@@ -316,19 +342,19 @@
         urxdata.rcvdata[0] = rdata6[0];
         BEMFq = urxdata.rxdata;
     }
-
+ 
     // reading heatsink and motor temp
-    else if(can2.read(msg) && msg.id == (MC_BASE+MC_TEMP)) {
+    else if(msg.id == (MC_BASE+MC_TEMP)) {
         for (int i = 0; i < msg.len; i++) {
           rdata7[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x24B, (char*) rdata7, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata7[7];
         urxdata.rcvdata[2] = rdata7[6];
         urxdata.rcvdata[1] = rdata7[5];
@@ -339,21 +365,20 @@
         urxdata.rcvdata[1] = rdata7[1];
         urxdata.rcvdata[0] = rdata7[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)) {
+    else if(msg.id == (MC_BASE+MC_AMPH)) {
         for (int i = 0; i < msg.len; i++) {
           rdata8[i] = msg.data[i];
         }
-
+ 
         // sending value to CAR can
         if (!can1.write(CANMessage(0x24E, (char*) rdata8, 8))) {
           pc.printf("Cannot write to CAN\n");
         }
-        wait(0.1);
-
+        wait(0.001);
+ 
         urxdata.rcvdata[3] = rdata8[7];
         urxdata.rcvdata[2] = rdata8[6];
         urxdata.rcvdata[1] = rdata8[5];
@@ -365,7 +390,8 @@
         urxdata.rcvdata[0] = rdata8[0];
         odometerValue = urxdata.rxdata;
     }
-
+    }
+ 
        if(alive % 100 == 0){
        printf("Motor board is running \n\r");
        printf(" Requested Motor Current: %f\n\r", current);
@@ -374,7 +400,7 @@
        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");
@@ -388,7 +414,7 @@
        printf("\r\n");
        printf(" Vq (V) = %f",vq);
        printf("\r\n");
-
+ 
        printf(" Id (A) = %f",Id);
        printf("\r\n");
        printf(" Iq (A) = %f",Iq);
@@ -405,8 +431,8 @@
        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