Revision 7:b539135656cb, committed 2021-12-01

Comitter:

lschwend

Date:

Wed Dec 01 23:14:47 2021 +0000

Parent:

6:7f39aa2c97da

Commit message:

-changed signs to make the legs work;

Changed in this revision

--- a/main.cpp	Tue Nov 30 22:20:16 2021 +0000
+++ b/main.cpp	Wed Dec 01 23:14:47 2021 +0000
@@ -169,8 +169,8 @@
     prev_current_des2 = current_des2; 
     
     
-    current3 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
-    velocity3 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
+    current3 = -(((float(motorShield.readCurrentC())/65536.0f)*30.0f)-15.0f);           // measure current
+    velocity3 = encoderC.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
     float err_c3 = current_des3 - current3;                                             // current errror
     current_int3 += err_c3;                                                             // integrate error
     current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max);      // anti-windup
@@ -190,8 +190,8 @@
     prev_current_des3 = current_des3; 
     
     
-    current4 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
-    velocity4 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
+    current4 = -(((float(motorShield.readCurrentD())/65536.0f)*30.0f)-15.0f);           // measure current
+    velocity4 = encoderD.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
     float err_c4 = current_des4 - current4;                                             // current errror
     current_int4 += err_c4;                                                             // integrate error
     current_int4 = fmaxf( fminf(current_int4, current_int_max), -current_int_max);      // anti-windup
@@ -204,9 +204,9 @@
         absDuty4 = duty_max;
     }    
     if (duty_cycle4 < 0) { // backwards
-        motorShield.motorCWrite(absDuty4, 1);
+        motorShield.motorDWrite(absDuty4, 1);
     } else { // forwards
-        motorShield.motorCWrite(absDuty4, 0);
+        motorShield.motorDWrite(absDuty4, 0);
     }             
     prev_current_des4 = current_des4;     
     
@@ -317,10 +317,10 @@
                 const float dth2= velocity2;
  
  
-                const float th3 = -angle3;
-                const float th4 = -angle4;
-                const float dth3= -velocity3;
-                const float dth4= -velocity4;
+                const float th3 = angle3;
+                const float th4 = angle4;
+                const float dth3= velocity3;
+                const float dth4= velocity4;
                 
                 
                 
@@ -454,8 +454,8 @@
                 float de_x = vxDes - dxFoot;
                 float de_y = vyDes - dyFoot;
                 
-                float xDes2 = b*cos(Omega*teff + phase) + x0; 
-                float yDes2 = a*sin(Omega*teff + phase) + y0;
+                float xDes2 = b*cos(-Omega*teff + phase) + x0; 
+                float yDes2 = a*sin(-Omega*teff + phase) + y0;
                 float vxDes2 = 0;//-b*Omega*sin(Omega*teff + phase);
                 float vyDes2 = 0; //a*Omega*cos(Omega*teff + phase); 
                 
@@ -478,7 +478,7 @@
                 current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;          
                 current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;   
                 current_des3 =  (Jx_th3*fx2+Jy_th3*fy2)/k_t;   
-                current_des4 = (Jx_th4*fx2+Jy_th4*fy2)/k_t
+                current_des4 = (Jx_th4*fx2+Jy_th4*fy2)/k_t;