Diff: main.cpp

Revision:

12:5be2001abe62

Parent:

11:bda77916d2ea

Child:

13:acd120520e80

--- a/main.cpp	Fri Oct 27 10:00:20 2017 +0000
+++ b/main.cpp	Fri Oct 27 13:43:28 2017 +0000
@@ -21,14 +21,16 @@
 // ---- Motor input and outputs ----
 PwmOut speed1(D5);
 PwmOut speed2(D6);
+PwmOut speedservo1(D11);
+//PwmOut speedservo2(D8);
 DigitalOut dir1(D4);
 DigitalOut dir2(D7);
 DigitalIn press(PTA4);
 DigitalOut ledred(LED_RED);
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
-DigitalOut ledstateswitch(D8);
-DigitalOut ledstatedef(D11);
+DigitalOut ledstateswitch(D3);
+DigitalOut ledstatedef(D2);
 AnalogIn pot(A2);
 AnalogIn pot2(A3);
 Encoder motor1(PTD0,PTC4);
@@ -51,7 +53,7 @@
 
 double setpoint = 6.28;//I am setting it to move through 180 degrees
 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
-double Kp = 500;// you can set these constants however you like depending on trial & error
+double Kp = 250;// you can set these constants however you like depending on trial & error
 double Ki = 100;
 double Kd = 0;
 
@@ -190,7 +192,7 @@
 {
     setpointreadout();
     angle = motor1.getPosition()/4200.00;   
-    angle2 = motor2.getPosition()/4200.00*6.28;
+    angle2 = motor2.getPosition()/4200.00;
    
     //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
     //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
@@ -211,9 +213,9 @@
     }
     //constraining to appropriate value
         if (pidTerm >= 0) {
-        dir1 = 1;// Forward motion
+        dir1 = 0;// Forward motion
     } else {
-        dir1 = 0;//Reverse motion
+        dir1 = 1;//Reverse motion
     }
     pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
     if(pidTerm_scaled >= 0.55f){
@@ -244,8 +246,10 @@
         
     last_error = error1;
     speed1 = pidTerm_scaled+0.45f;
+    //speedservo1 = speed1;
     last_error2 = error2;
     speed2 = pidTerm_scaled2+0.45f;
+    //speedservo2 = speed2;
 }
 
 void maintickerfunction()