Diff: main.cpp

Revision:

5:a2e3d0213315

Parent:

4:0ce97b9fde37

Child:

6:95dc3761f64a

--- a/main.cpp	Thu Aug 08 20:03:14 2019 +0000
+++ b/main.cpp	Mon Dec 09 23:41:16 2019 +0000
@@ -25,11 +25,14 @@
 
 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 tilt_angle = 1.35f;
+    float tilt_angle = 13.5f;
     
     float t = DT*loop_counter;
     if(t<1)
@@ -76,7 +79,7 @@
     motors[0].control.kd = .5f;
     motors[0].control.kp = 2.0f;
     
-    //motors[1].control.p_des = 2*sin(.01f*loop_counter);
+    motors[1].control.p_des = 2*sin(.01f*loop_counter);
     motors[1].control.kd = 1.0f;
     motors[1].control.kp = 20.0f;
     /*                              */
@@ -91,7 +94,8 @@
     
     
     //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("%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
 }
 
@@ -103,14 +107,19 @@
     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,2};                 // List of motor CAN ID's
     init_motors(ids);                           // Initialize the list of motors
     
-    enable_motor(&motors[0], &can);             // Enable first motor
+    enable_motor(&motors[0], &can);             // Enable motors
     enable_motor(&motors[1], &can);
+    
+    zero_motor(&motors[0], &can);               //Zero motors
+    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)