Revision 2:ab65342daec8, committed 2021-07-20

Comitter:

adimmit

Date:

Tue Jul 20 19:36:33 2021 +0000

Parent:

1:6b03278584b2

Commit message:

latest

Changed in this revision

--- a/main.cpp	Mon Jun 21 19:56:09 2021 +0000
+++ b/main.cpp	Tue Jul 20 19:36:33 2021 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include "math_ops.h"
 
-#define MIN -400.0f
-#define MAX 400.0f
+#define MIN -5.0f
+#define MAX 5.0f
 
 /*
    This basic example just shows how to read the ADC internal channels raw values.
@@ -13,11 +13,14 @@
 //setup SERIAL
 Serial       pc(PA_2, PA_3);
 
-//setup scaling
-float TORQUE_SCALING = 0.0550; //V/A
+//setup analog inputs
+AnalogIn torque_sense(PC_0);
+float SENSE_CONV_FACTOR = 10.0; //-10v to 10v --> -100Nm to 100Nm
+float SYSTEM_VOLTAGE = 3.3; //volts
 
-//setup analog inputs
-AnalogIn hv_current(PC_0);
+//moving average filter
+#define M_POINTS 2
+float points[M_POINTS] = {0.0, 0.0};
 
 //setup CAN
 CAN          can(PA_11, PA_12, 1000000);  // CAN Rx pin name, CAN Tx pin name
@@ -28,21 +31,24 @@
 volatile bool           msgAvailable = false;
 Ticker loop;
 
+float MOVING_AVERAGE(float new_point) {
+        //first let's update the object points and calculate the SUM
+        float SUM = 0.0;
+        int i;
+        for ( i = 0; i < M_POINTS-1; i = i + 1) {
+               points[i] = points[i+1];
+               SUM = SUM + points[i];
+        }
+        points[M_POINTS-1] = new_point;
+        SUM = SUM + new_point;
+        return SUM / float(M_POINTS);
+    }
+
 void pack_cmd(CANMessage * msg, float f1){
      /// convert floats to unsigned ints ///
      uint16_t f1_int = float_to_uint(f1, MIN, MAX, 16);            
-     //uint16_t f2_int = float_to_uint(f2, MIN, MAX, 16);
-     /// pack ints into the can buffer ///
      msg->data[0] = f1_int>>8;                                       
      msg->data[1] = f1_int&0xFF;
-     /*
-     msg->data[2] = f2_int>>8;
-     msg->data[3] = f2_int&0xFF;
-     msg->data[4] = 0x00;
-     msg->data[5] = 0x00;
-     msg->data[6] = 0x00;
-     msg->data[7] = 0x00;
-     */
      }
 
 int main()
@@ -54,21 +60,20 @@
     wait(.01);
     
     tauMsg.len = 2;                        //transmit 4 bytes
-    tauMsg.id = 0x2;                       //higher priority                
+    tauMsg.id = 0x1;                       //higher priority                
     
     while(1){
-        //wait(1); //GET RID OF THIS WAIT LATER
-        float hv_cur = (hv_current.read()*SYSTEM_VOLTAGE-VCC/2.0) / HV_CURRENT_SCALING;                                     //PUBLISH
-        float lv_cur = (lv_current.read()*SYSTEM_VOLTAGE-VCC/2.0) / LV_CURRENT_SCALING;                                     //PUBLISH
-        float vol = (voltage_sense.read()*SYSTEM_VOLTAGE)*VOL_SENSE_SCALING; //INACCURATE ON THE HARDWARE SIDE!!!           //PUBLISH
+        //wait(0.0003); //MAKE SURE IT's 1-3k rate, higher we will fill the CAN buffer
+        float v_tau = torque_sense.read()*SYSTEM_VOLTAGE;                                  //PUBLISH
+        float tau = v_tau;
         
-        float V_OUT_EXT = (ext_current_A.read()*SYSTEM_VOLTAGE);
-        float V_REF_EXT = (ext_current_B.read()*SYSTEM_VOLTAGE);
+        //calc the moving average
+        //tau = MOVING_AVERAGE(tau);
         
-        float ext_cur = (V_OUT_EXT*SYSTEM_VOLTAGE-V_REF_EXT*SYSTEM_VOLTAGE) / EXT_CURRENT_SCALING;                            //PUBLISH
+        //pc.printf("%f\n", tau);
         
-        //now we have to publish the four floats to the CAN BUS
-        pack_cmd(&tauMsg, hv_cur);
+        //now we have to publish the float to the CAN BUS
+        pack_cmd(&tauMsg, tau);
         can.write(tauMsg);
         wait(.00002);
     }