Diff: main.cpp

Revision:

4:0ce97b9fde37

Parent:

3:f0d054d896f9

--- a/main.cpp	Thu Aug 08 17:39:17 2019 +0000
+++ b/main.cpp	Thu Aug 08 20:03:14 2019 +0000
@@ -12,7 +12,7 @@
 
 Ticker loop;                                // Control loop interrupt handler
 int loop_counter = 0;
-#define DT                  .01             // Control loop period
+#define DT                  .01f             // Control loop period
 
 #define N_MOTORS             2              // Number of motors on the can bus
 MotorStruct motors[N_MOTORS];               // Create a list of the motors attached
@@ -22,15 +22,67 @@
 void init_motors(int ids[N_MOTORS]);
 /*                                       */
 
+
 void control()
 {
     /* Your control loop goes here.  */
     /* Update torques, position/velocity setpoints, etc */
-    motors[0].control.p_des = 10.0f*sin(.01f*loop_counter);
+    //motors[0].control.p_des = -40 + 40.0f*cos(.01f*loop_counter);
+
+    float tilt_angle = 1.35f;
+    
+    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 = 1.0f;
+    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]);
@@ -38,7 +90,8 @@
     }
     
     
-    printf("%f\n\r", motors[0].state.position);           // This will print to the computer.  Usefull for debugging
+    //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);
     loop_counter++;     // Increment loop counter
 }
 
@@ -54,9 +107,10 @@
     init_motors(ids);                           // Initialize the list of motors
     
     enable_motor(&motors[0], &can);             // Enable first motor
-    //wait(1);                                    // Wait 1 second
+    enable_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)