asdfasdf
Dependencies: HIDScope MODSERIAL PID QEI biquadFilter mbed
Fork of cpfromralph by
Diff: main.cpp
- Revision:
- 36:2fbc53a2747c
- Parent:
- 35:82b432f5aef7
- Child:
- 37:6ac3e70787d1
diff -r 82b432f5aef7 -r 2fbc53a2747c main.cpp --- a/main.cpp Fri Nov 03 01:33:27 2017 +0000 +++ b/main.cpp Fri Nov 03 01:50:10 2017 +0000 @@ -521,7 +521,7 @@ double beta = inversekinematics2(x,y); setpoint1 = alpha; setpoint2 = beta; - wait(0.1f); + } // This part checks for left biceps contractions only else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){ @@ -535,7 +535,7 @@ double beta = inversekinematics2(x,y); setpoint1 = alpha; setpoint2 = beta; - wait(0.1f); + } // This part checks for left lower arm contractions only else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){ @@ -549,7 +549,7 @@ double beta = inversekinematics2(x,y); setpoint1 = alpha; setpoint2 = beta; - wait(0.1f); + } // This part checks for right lower arm contractions only else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){ @@ -563,7 +563,7 @@ double beta = inversekinematics2(x,y); setpoint1 = alpha; setpoint2 = beta; - wait(0.2f); + } /* // This part checks for both lower arm contractions only else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){ @@ -658,7 +658,7 @@ last_error2 = new_error2; } -*/ + */ // Motor control // // @@ -666,15 +666,16 @@ void SetMotor1() { //PIDcalculation1(); filter(); + count1 = Encoder1.getPulses(); angle1 += (0.0981 * count1); if (angle1<setpoint1){ - motor1direction = 0; // counterclockwise rotation + motor1direction = 1; // counterclockwise rotation } else { - motor1direction = 1; // clockwise rotation + motor1direction = 0; // clockwise rotation } - while (angle1<setpoint1 || angle1>setpoint1) { - if (angle2<setpoint2 || angle2>setpoint2) { + while (angle1<setpoint1 /*|| angle1>setpoint1*/) { + if (angle1<setpoint1 /*|| angle1>setpoint1*/) { motor1pwm = 0.5; } else if (angle1 == setpoint1){ @@ -686,15 +687,16 @@ void SetMotor2() { filter(); //PIDcalculation2(); + count2 = Encoder2.getPulses(); angle2 += (0.0981 * count2); if (angle2<setpoint2){ - motor2direction = 0; // counterclockwise rotation + motor2direction = 1; // counterclockwise rotation } else { - motor2direction = 1; // clockwise rotation + motor2direction = 0; // clockwise rotation } - while (angle2<setpoint2 || angle2>setpoint2) { - if (angle2<setpoint2 || angle2>setpoint2) { + while (angle2<setpoint2 /*|| angle2>setpoint2*/) { + if (angle2<setpoint2 /*|| angle2>setpoint2*/) { motor2pwm = 0.5; } else if (angle2 == setpoint2){