kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar2 by
Diff: PROJECT_main.cpp
- Revision:
- 10:6bf3e25f020a
- Parent:
- 9:0bc7f83b761e
- Child:
- 11:b517e73a98ab
diff -r 0bc7f83b761e -r 6bf3e25f020a PROJECT_main.cpp --- a/PROJECT_main.cpp Mon Nov 03 20:17:43 2014 +0000 +++ b/PROJECT_main.cpp Mon Nov 03 21:31:40 2014 +0000 @@ -13,7 +13,7 @@ //Define objects -HIDScope scope(2); +HIDScope scope(4); AnalogIn emg0(PTB1); //Analog input biceps AnalogIn emg1(PTB2); //Analog input triceps @@ -134,10 +134,16 @@ void setlooptimerflag(void) { looptimerflag = true; +} + +Ticker hid; + +void hidscope(void){ scope.set(0, motor2.getPosition()*omrekenfactor2); - scope.set(0, motor1.getPosition()*omrekenfactor1); - scope.send(); - + scope.set(1, setpoint2); + scope.set(2, motor1.getPosition()*omrekenfactor1); + scope.set(3, setpoint1); + scope.send(); } void keep_in_range(float * in, float min, float max) @@ -272,6 +278,7 @@ int main() { + hid.attach(hidscope, 0.01); pc.baud(115200); //baudrate instellen log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz looptimer.attach(setlooptimerflag,TSAMP); @@ -526,6 +533,8 @@ setpoint2=0; integral1 = integral = 0; previouserror1 = previouserror = 0; + + while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) while(!looptimerflag) {} @@ -547,11 +556,13 @@ break; } if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1 - setpoint2 = angle; + setpoint2 = angle; + count = 0; + state=2; } - if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.04) { + /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) { state = 2; - } + }*/ break; case 2: @@ -561,10 +572,11 @@ count = 0; state = 3; } + break; case 3: switch (force) { case 1: - setpoint1 += 0.4*TSAMP; //6.8*TSAMP; + setpoint1 += 2.5*TSAMP; //6.8*TSAMP; break; case 2: setpoint1 += 0.4*TSAMP; //7.4*TSAMP; @@ -573,18 +585,18 @@ setpoint1 += 0.4*TSAMP; //8.0*TSAMP; break; } - if(abs(motor1.getPosition()*omrekenfactor1)>2.1) { + if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) { state = 4; } break; case 4: setpoint2 -= 0.25*TSAMP; - if(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) { // op 0 draait hij mogelijk in de arm + if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm state = 5; } break; case 5: - setpoint1 -= 0.25*TSAMP; + setpoint1 -= 0.5*TSAMP; if(setpoint1 < 0) { state = 6; } @@ -610,7 +622,7 @@ previouserror1 = controlerror1; keep_in_range(&pwm1, -1,1); - pwm_motor1.write(abs(pwm1)); + pwm_motor1.write(fabs(pwm1)); if(pwm1 > 0) { motor1dir = 1; } else { @@ -626,7 +638,7 @@ previouserror = controlerror; keep_in_range(&pwm, -1,1); - pwm_motor2.write(abs(pwm)); + pwm_motor2.write(fabs(pwm)); if(pwm > 0) { motor2dir = 1; } else {