Diff: main.cpp

Revision:

6:95dc3761f64a

Parent:

5:a2e3d0213315

Child:

7:e4fc29c6f014

--- a/main.cpp	Mon Dec 09 23:41:16 2019 +0000
+++ b/main.cpp	Wed Feb 19 04:52:01 2020 +0000
@@ -1,138 +1,142 @@
-
 #define CAN_ID 0x0
-
 #include "mbed.h"
 #include "math_ops.h"
 #include "MotorModule.h"
 
+typedef union _data {
+  float f;
+  char  s[4];
+} myData;
 
 
 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
+myData  dat1;
+myData  dat2;
+myData  dat3;
+myData  dat4;
 
-#define N_MOTORS             2              // Number of motors on the can bus
+Ticker loop_ctrl;                                // Control loop interrupt handler
+Ticker loop_msg;
+
+#define DT_ctrl             .001f             // Control loop period
+#define DT_msg              .01f             // 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]);
-/*                                       */
 
-
-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);
+int loop_counter = 0;
+int newprint = 1;
+int nskip = 5;  
+int skip_counter = 0;
 
-    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
+float t = 0.0;
+float A=  0.5;
+float f = 1.0;
+double mycur = 0.2;
+bool flagCtrl = true;
+
+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<20){
+            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
+
     
-    int ids[N_MOTORS] = {1,2};                 // List of motor CAN ID's
+    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);
     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.1;
+            }
+            if(c == 'r') {
+                mycur = mycur - 0.1;
+            }
+
+        }
+        if(newprint == 1){
+            printf("%f \t%f \t%f \r\n", t, motors[0].state.position, motors[0].state.current);
+            //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 */
 {