Kim Bruil / Mbed 2 deprecated r_robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

Diff: main.cpp

Revision:
8:681151d89a75
Parent:
7:b09bfde57cba
Child:
9:19dafcb4a098
diff -r b09bfde57cba -r 681151d89a75 main.cpp
--- a/main.cpp	Wed Oct 26 08:30:44 2016 +0000
+++ b/main.cpp	Wed Oct 26 12:04:11 2016 +0000
@@ -47,13 +47,13 @@
 */
 DigitalOut motor1dir(D7);
 PwmOut motor1pwm(D6);
-QEI motor1sense(D13,D12,NC, 8400);
+QEI motor1sense(D13,D12,NC, 8400, QEI::X4_ENCODING);
 double motor1pos=0;
 double motor1int=0;
 double motor1olderr=0;
 DigitalOut motor2dir(D4);
 PwmOut motor2pwm(D5);
-QEI motor2sense(D10,D11,NC, 8400);
+QEI motor2sense(D10,D11,NC, 8400, QEI::X4_ENCODING);
 double motor2pos=0;
 double motor2int=0;
 double motor2olderr=0;
@@ -487,6 +487,7 @@
 }
 
 void setmotor1(float val){
+    val = -val;
     motor1dir.write(val>=0?1:0);
     motor1pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75));//pow((float)fabs(val),(float)3/4.));
 }
@@ -513,7 +514,7 @@
     
     if (flag_PID){
         flag_PID = false;
-        motor1pos = (double)motor1sense.getPulses()*(2*M_PI/8400.0);
+        motor1pos = -(double)motor1sense.getPulses()*(2*M_PI/8400.0);
         motor2pos = (double)motor2sense.getPulses()*(2*M_PI/8400.0);
         m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, motor1bqc_derivative, dt, 1, 0.2, 0.1);
         m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, motor2bqc_derivative, dt, 1, 0.2, 0.1);