Diff: main.cpp

Revision:

6:09738d84a005

Parent:

5:a2e3d0213315

--- a/main.cpp	Mon Dec 09 23:41:16 2019 +0000
+++ b/main.cpp	Fri Feb 12 22:50:35 2021 +0000
@@ -1,138 +1,246 @@
-
+#include "HX711.h"
 #define CAN_ID 0x0
-
 #include "mbed.h"
 #include "math_ops.h"
 #include "MotorModule.h"
 
 
+typedef union _data {
+  float f;
+  char  s[4];
+} myDatafs;
 
+
+
+
+
+
+HX711        loadCell( PB_10, PA_8 );
 Serial       pc(PA_2, PA_3);                // Serial port to the computer
 CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
 
-Ticker loop;                                // Control loop interrupt handler
-int loop_counter = 0;
-#define DT                  .01f             // Control loop period
+myDatafs  dat1;
+myDatafs  dat2;
+myDatafs  dat3;
+myDatafs  dat4;
+
+Ticker loop_ctrl;                                // Control loop interrupt handler
+Ticker loop_msg;
+Ticker newReading;
 
-#define N_MOTORS             2              // Number of motors on the can bus
+DigitalOut myled1(LED1);
+DigitalOut myled2(PB_3);
+DigitalOut myled3(PA_10);
+
+HX711::HX711_status_t aux;
+HX711::Vector_count_t myData;
+HX711::Vector_mass_t calcMass;
+HX711::Vector_voltage_t calcVolt;
+
+#define DT_ctrl             .002f             // Control loop period
+#define DT_msg              .2f             // msg loop period    
+#define N_MOTORS            1               // Number of motors on the can bus
+
 MotorStruct motors[N_MOTORS];               // Create a list of the motors attached
 
 /* Communication functions.  Do not touch */
 void onMsgReceived();
 void init_motors(int ids[N_MOTORS]);
-/*                                       */
 
+int loop_counter = 0;
+int newprint = 1;
+int nskip = 5;  
+int skip_counter = 0;
 
-void control()
-{
-    //set the other values to see if it keeps the motor from freaking out
-    motors[0].control.kp = 1;
-    motors[1].control.kp = 1;
-    /* Your control loop goes here.  */
-    /* Update torques, position/velocity setpoints, etc */
-    //motors[0].control.p_des = -40 + 40.0f*cos(.01f*loop_counter);
+float t = 0.0;
+float A=  0.5;
+float f = 1.0;
+float mycur = 0.1;
+bool flagCtrl = true;
 
-    float tilt_angle = 13.5f;
-    
-    float t = DT*loop_counter;
-    if(t<1)
-    {
-        motors[1].control.p_des = -tilt_angle*t; // dump left to -1.5
-        motors[0].control.p_des = 0;
-    }   
-    else if(t>1 && t<3)
-    {
-        motors[1].control.p_des = tilt_angle*(t-1.0f) - tilt_angle; // dump right from -1.5 to 1.5
-        motors[0].control.p_des = 0;
-    }   
-    else if(t>3 && t<4)
-    {
-        motors[1].control.p_des = -tilt_angle*(t-3.0f) + tilt_angle;
-        motors[0].control.p_des = 0;
-    }   // center from 1.5 to 0
-    else if (t>4 && t<7)
-    {
-        motors[1].control.p_des = 0;
-        motors[0].control.p_des = -40.0f+40.0f*cos((t-4.0f));
-    }
-    else if(t>7 && t<8)
-    {
-        motors[1].control.p_des = -tilt_angle*(t-7.0f); // dump left to -1.5
-        motors[0].control.p_des = -80;
-    }   
-    else if(t>8 && t<10)
-    {
-        motors[1].control.p_des = tilt_angle*(t-8.0f) - tilt_angle; // dump right from -1.5 to 1.5
-        motors[0].control.p_des = -80;
-    }   
-    else if(t>10 && t<11)
-    {
-        motors[1].control.p_des = -tilt_angle*(t-10.0f) + tilt_angle;
-        motors[0].control.p_des = -80;
-    }   // center from 1.5 to 0
-    else if (t>11 && t<14)
-    {
-        motors[1].control.p_des = 0;
-        motors[0].control.p_des = -40.0f-40.0f*cos((t-11.0f));
-    }
-    
-    motors[0].control.kd = .5f;
-    motors[0].control.kp = 2.0f;
-    
-    motors[1].control.p_des = 2*sin(.01f*loop_counter);
-    motors[1].control.kd = 1.0f;
-    motors[1].control.kp = 20.0f;
-    /*                              */
-    
-    if(t>14){loop_counter = 0;}
-    
-    for(int i = 0; i<N_MOTORS; i++)
-    {
-        pack_cmd(&motors[i]);
-        can.write(motors[i].txMsg);
-    }
-    
-    
-    //printf("%f  %f\n\r", motors[0].control.p_des, motors[1].control.p_des);           // This will print to the computer.  Usefull for debugging
-//    printf("%f  %f\n\r", motors[0].state.position, motors[1].state.position);
-    printf("1 \t pos1: %f \t des1: %f \t pos2: %f \t des2: %f\n\r", motors[0].state.position, motors[0].control.p_des, motors[1].state.position, motors[1].control.p_des);
-    loop_counter++;     // Increment loop counter
+void readDATA ( void )
+{
+    myled2 = 1;
+
+    aux =       loadCell.HX711_ReadRawData ( HX711::CHANNEL_A_GAIN_128, &myData, 1 );
+    calcMass =  loadCell.HX711_CalculateMass ( &myData, 0.500, HX711::HX711_SCALE_g );
+    calcVolt =  loadCell.HX711_CalculateVoltage ( &myData, 5.0 );
+
+   // pc.printf( "RawData: %ld Mass: %0.3f g Voltage: %0.5f mV\r\n", (uint32_t)myData.myRawValue, calcMass.myMass, 1000*calcVolt.myVoltage );
+
+    myled2 = 0;
+}
+
+void printnew(){
+    newprint = 1;
 }
 
+void control(){ 
+    t = DT_ctrl*loop_counter;  //time in seconds
+    if (flagCtrl) {
+        // if control is ON
+        if (t<1) {
+            motors[0].control.p_des = 0;
+            motors[0].control.v_des = 0;
+            motors[0].control.kp = 0;
+            motors[0].control.kd = 0;
+        }
+        if (1<=t){
+            motors[0].control.i_ff = mycur;
+            motors[0].control.kd = 0;
+            motors[0].control.kp = 0;
+        }
+        for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
+            pack_cmd(&motors[i]);
+            can.write(motors[i].txMsg);
+        }
+    } else {
+        // if control is OFF
+        motors[0].control.i_ff = 0.0f;  //all set to zero (no control)
+        motors[0].control.kp = 0.0f;
+        motors[0].control.kd = 0.0f;
+        
+        loop_counter = 0.0f;            //re-zero loop timing 
+        
+        for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
+            pack_cmd(&motors[i]);
+            can.write(motors[i].txMsg);
+        }
+    }
+    loop_counter++;     // increase loop counter
+}
 
 int main() 
 {
-    
+    /* Setup & Initialization */
     pc.baud(921600);                            // Set baud rate for communication over USB serial
     can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
     can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
+
+    myled1 = 0;    myled2 = 0;    myled3 = 0;
+    myled2 = 1;    myled3 = 1;    wait(0.5);    myled2 = 0;    myled3 = 0;    wait(0.5);
+    myled2 = 1;    myled3 = 1;    wait(0.5);    myled2 = 0;    myled3 = 0;    wait(0.5);
     
-    int ids[N_MOTORS] = {1,2};                 // List of motor CAN ID's
+    //Reset and wake the device up 
+    aux = loadCell.HX711_PowerDown();
+    aux = loadCell.HX711_Reset();
+    
+    myled2 =1;    wait(0.2);    myled2 =0;  wait(0.2);        myled2 =1;    wait(0.2);    myled2 =0;  wait(0.2);    
+    myled2 =1;    wait(0.2);    myled2 =0;  wait(0.2);        myled2 =1;    wait(0.2);    myled2 =0;  wait(0.2);    
+
+    //CALIBRATION time start! 
+    // 1. REMOVE THE MASS ON THE LOAD CELL ( ALL LEDs OFF ). Read data without any mass on the load cell 
+    pc.printf("1. Self Mass Measurement beginning, remove all\r\n");
+    pc.printf("count 2\r\n");
+    wait(1);
+    pc.printf("count 1\r\n");
+    wait(1);
+    pc.printf("GO\r\n");
+    
+    aux = loadCell.HX711_ReadData_WithoutMass( HX711::CHANNEL_A_GAIN_128, &myData, 20);
+    
+    pc.printf("1. calib done.\r\n");
+    wait(1.5);
+    myled3 =1;    wait(0.2);    myled3 =0;  wait(0.2);      myled3 =1;    wait(0.2);    myled3 =0;  wait(0.2);
+    myled3 =1;    wait(0.2);    myled3 =0;  wait(0.2);      myled3 =1;    wait(0.2);    myled3 =0;  wait(0.2);
+    
+    //2. PUT A KNOWN MASS ON THE LOAD CELL ( JUST LED1 ON ). Read data with an user-specified calibration mass 
+    pc.printf("2. Put on a Calibrated Mass, Beginning, \r\n");
+    pc.printf("count 3\r\n");
+    wait(1);
+    pc.printf("count 2\r\n");
+    wait(1);
+    pc.printf("count 1\r\n");
+    wait(1);
+    pc.printf("GO\r\n");
+    wait(1);
+    
+    aux = loadCell.HX711_ReadData_WithCalibratedMass ( HX711::CHANNEL_A_GAIN_128, &myData, 20 ); 
+    
+    pc.printf("2. Calibration is done.\r\n");
+    wait(1);
+
+//    //OPTIONAL - TARING
+//    myled3 = 1; 
+//    aux = loadCell.HX711_SetAutoTare ( HX711::CHANNEL_A_GAIN_128, 0.48, HX711::HX711_SCALE_kg, &myData, 5 ); 
+//    myled3 = 0;
+
+    myled1 = 0;    myled2 = 0;    myled3 = 0;
+    myled2 = 1;    myled3 = 1;    wait(0.5);    myled2 = 0;    myled3 = 0;    wait(0.5);
+    myled2 = 1;    myled3 = 1;    wait(0.5);    myled2 = 0;    myled3 = 0;    wait(0.5);
+
+//  All set up, and ready to go. Measure what ever you want    
+    pc.printf("3. RT Measure start.\r\n");
+    pc.printf("count 3\r\n");
+    wait(1);
+    pc.printf("count 2\r\n");
+    wait(1);
+    pc.printf("count 1\r\n");
+    wait(1);
+    pc.printf("GO\r\n");
+    
+    newReading.attach( &readDATA, DT_msg ); // the address of the function to be attached ( readDATA ) and the interval ( 0.5s ) ( JUST LED4 BLINKING )
+    pc.printf("LoadCell Interrupt Enabled\r\n");
+//
+    
+    int ids[N_MOTORS] = {1};                 // List of motor CAN ID's
     init_motors(ids);                           // Initialize the list of motors
     
     enable_motor(&motors[0], &can);             // Enable motors
-    enable_motor(&motors[1], &can);
+    //enable_motor(&motors[1], &can);
     
     zero_motor(&motors[0], &can);               //Zero motors
-    zero_motor(&motors[1], &can);
+    //zero_motor(&motors[1], &can);
+    pc.printf("Motor enabled \r\n");
     wait(1);                                    // Wait 1 second
     
     //disable_motor(&motors[0], &can);            // Disable first motor
     //disable_motor(&motors[1], &can); 
     
-    loop.attach(&control, DT);                 // Start running the contorl interrupt at 1/DT Hz
-        
-    while(1) 
-    {
-        // Usuallly nothing should run here.  Instead run control in the interrupt.
+    /* Interrupt Enables */
+    loop_ctrl.attach(&control, DT_ctrl);                 // Start running the contorl interrupt at 1/DT Hz
+    loop_msg.attach(&printnew, DT_msg);    
+    
+    while(1) {
+        if (pc.readable()) {
+            char c = pc.getc();
+            if(c == 's') {
+                flagCtrl = false;
+                printf("s");
+            }
+            if(c == 'g') {
+                flagCtrl = true;
+                printf("g");
+            }
+            if(c == 'e') {
+                mycur = mycur +0.02;
+            }
+            if(c == 'r') {
+                mycur = mycur - 0.02;
+            }
+            if(c == 'd') {
+                mycur = mycur +0.1;
+            }
+            if(c == 'f') {
+                mycur = mycur - 0.1;
+            }
+
+        }
+        if(newprint == 1){
+            pc.printf("%.3f    %.3f   %.3f\r\n", mycur, motors[0].state.current, calcMass.myMass);
+            //pc.printf( "RawData: %ld Mass: %0.3f g Voltage: %0.5f mV\r\n", (uint32_t)myData.myRawValue, , 1000*calcVolt.myVoltage );
+            
+            //pc.putc(motors[0].state.position);
+            //pc.write(&motors[0].state.position);
+            //dat1.f = motors[0].state.position;
+            //pc.printf("%c%c%c%c", dat1.s[0], dat1.s[1], dat1.s[2], dat1.s[3]);
+            newprint = 0;
+        }
     }
-        
 }
     
-
 /* low-level communication functoins below.  Do not touch */
 
-
  void onMsgReceived() 
 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
 {