Diff: main.cpp

Revision:

13:f92e918af729

Parent:

12:0cf4e70f6b8e

Child:

14:6ecf2b986a4b

diff -r 0cf4e70f6b8e -r f92e918af729 main.cpp
--- a/main.cpp	Mon Oct 24 12:34:45 2016 +0000
+++ b/main.cpp	Mon Oct 24 14:28:04 2016 +0000
@@ -36,9 +36,9 @@
 const double transmissionShoulder =94.4/40.2;
 const double transmissionElbow = 1.0;
 // controller constants 
-const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100;
+const double m1_Kp = 0.000048765659063912, m1_Ki = 0.0000228295351674407, m1_Kd = 0.0000255784613247063, m1_N = 54.5397025421619;
 double m1_v1 = 0, m1_v2 = 0; // Memory variables
-const double m1_Ts = 0.01; // Controller sample time
+const double m1_Ts = 0.001; // Controller sample time
 double v1 = 0;
 double v2 = 0;
 // position variable
@@ -85,24 +85,26 @@
     radians = (pulses / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
 //     scope.set(velocityChannel,pulses);
 //     scope.send();
+
   
 }
   
  // Next task, measure the error and apply the output to the plant
  void motor1_Controller(double radians)
  {
- double reference = 2*pi;
+ double reference = 0.5*pi;
  volatile double error1 = reference - radians;
  motor1 = PID( error1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, v1, v2 );
 //pc.printf("%d\n\r",motor1);
 //scope.set(velocityChannel,motor1);
 //scope.send();
+pc.printf("error1= %d\n\r",error1);
  }
   
   
 void control(double motor1)
 {
-if(abs(motor1)>1.0)
+if(abs(motor1)>0.000015)
         {
         motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
         }
@@ -122,7 +124,7 @@
 //****************MAIN FUNCTION*********************************
 int main()
 {motor1MagnitudePin.period(1.0/1000.0);
-t1.attach(&fn1_activate, 0.1f);
+t1.attach(&fn1_activate, 0.0001f);
 t2.attach(&fn2_activate, 0.0001f);
 t3.attach(&fn3_activate, 0.0001f);
 pc.baud(115200);