Diff: main.cpp

Revision:

13:41fc5b6262c0

Parent:

12:55b76c319364

Child:

14:8aa57a7c5b30

--- a/main.cpp	Tue Oct 29 21:00:06 2019 +0000
+++ b/main.cpp	Tue Oct 29 21:33:42 2019 +0000
@@ -25,7 +25,7 @@
 float Ts = 0.01;                                //Sample time
 float deg2rad=0.0174532;                        //Conversion factor degree to rad
 float rad2deg=57.29578;                         //Conversion factor rad to degree
-float motor1angle=(-135.0-10.0)*deg2rad*5.5;    //Measured angle motor 1
+float motor1angle=(-140.0-10.0)*deg2rad*5.5;    //Measured angle motor 1
 float motor2angle=(-10.0)*deg2rad*2.75;         //Measured angle motor 2
 float motor1offset;                             //Offset bij calibratie
 float motor2offset;
@@ -44,7 +44,7 @@
 //Motorcontrol
 bool motordir1;
 bool motordir2;
-float motor1ref=(-135.0-10.0)*deg2rad*5.5;
+float motor1ref=(-140.0-10.0)*deg2rad*5.5;
 float motor2ref=(-10.0)*deg2rad*2.75;
 double controlsignal1;
 double controlsignal2;
@@ -74,7 +74,7 @@
 float Vx=0.0;                                   //Desired linear velocity x direction
 float Vy=0.0;                                   //Desired linear velocity y direction
 float q1=-10.0f*deg2rad;                          //Angle of first joint [rad]
-float q2=-135.0f*deg2rad;                       //Angle of second joint [rad]
+float q2=-140.0f*deg2rad;                       //Angle of second joint [rad]
 float q1dot;                                    //Velocity of first joint [rad/s]
 float q2dot;                                    //Velocity of second joint [rad/s]
 float l1=26.0;                                  //Distance base-link [cm]
@@ -221,13 +221,13 @@
     
     if (q1<0.0f){
         q1=0.0;}
-    else if (q1>90.0f*deg2rad){
-        q1=90.0f*deg2rad;}    
+    else if (q1>65.0f*deg2rad){
+        q1=65.0f*deg2rad;}    
     else{
         q1=q1;}
     
-    if (q2>-45.0*deg2rad){
-        q2=-45.0*deg2rad;}
+    if (q2>-50.0*deg2rad){
+        q2=-50.0*deg2rad;}
     else if (q2<-135.0*deg2rad){
         q2=-135.0*deg2rad;}
     else{
@@ -241,7 +241,7 @@
 
 void motorControl(){
     counts1= counts1af-counts1offset;
-    motor1angle = (counts1 * factorin / gearratio)-((135.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
+    motor1angle = (counts1 * factorin / gearratio)-((140.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor1error=motor1ref-motor1angle;         
     if (controlsignal1<0){