Aukie Hooglugt
kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of
PROJECT_incl_regelaar
by 
Aukie Hooglugt
Revision 11:b517e73a98ab, committed 2014-11-03
- Comitter:
- Hooglugt
- Date:
- Mon Nov 03 22:16:30 2014 +0000
- Parent:
- 10:6bf3e25f020a
- Child:
- 12:b09b7fe5550c
- Child:
- 16:a0a39512bd47
- Commit message:
- opzoek naar motor1error (viktor)
Changed in this revision
| PROJECT_main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/PROJECT_main.cpp Mon Nov 03 21:31:40 2014 +0000
+++ b/PROJECT_main.cpp Mon Nov 03 22:16:30 2014 +0000
@@ -13,7 +13,7 @@
//Define objects
-HIDScope scope(4);
+HIDScope scope(5);
AnalogIn emg0(PTB1); //Analog input biceps
AnalogIn emg1(PTB2); //Analog input triceps
@@ -139,10 +139,7 @@
Ticker hid;
void hidscope(void){
- scope.set(0, motor2.getPosition()*omrekenfactor2);
- scope.set(1, setpoint2);
- scope.set(2, motor1.getPosition()*omrekenfactor1);
- scope.set(3, setpoint1);
+
scope.send();
}
@@ -278,7 +275,7 @@
int main()
{
- hid.attach(hidscope, 0.01);
+ hid.attach(hidscope, 0.01);
pc.baud(115200); //baudrate instellen
log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz
looptimer.attach(setlooptimerflag,TSAMP);
@@ -533,122 +530,247 @@
setpoint2=0;
integral1 = integral = 0;
previouserror1 = previouserror = 0;
-
-
+
+
while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
- while(!looptimerflag)
- {}
+ while(!looptimerflag);
looptimerflag = false; //clear flag
+ setpoint1=0;
+ setpoint2=0;
+ integral1 = integral = 0;
+ previouserror1 = previouserror = 0;
+
+
+
+// FORMAT_CODE_START
+
+ scope.set(0, motor2.getPosition()*omrekenfactor2);
+ scope.set(1, setpoint2);
+ scope.set(2, motor1.getPosition()*omrekenfactor1);
+ scope.set(3, setpoint1);
+ scope.set(4, state);
- switch(state) {
- case 1:
- setpoint1=0;
- setpoint2 += 0.4*TSAMP;
- switch (direction) {
- case 1:
- angle = 0.436332313+0.197222205; //(0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel
- break;
- case 2:
- angle = 0.436332313;
- break;
- case 3:
- angle = 0.436332313-0.197222205;
- break;
- }
- if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1
- setpoint2 = angle;
- count = 0;
- state=2;
+ switch(state) {
+ case 1: {
+ setpoint1=0;
+ setpoint2 += 0.4*TSAMP;
+ switch (direction) {
+ case 1:
+ angle = 0.436332313+0.197222205; //(0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel
+ break;
+ case 2:
+ angle = 0.436332313;
+ break;
+ case 3:
+ angle = 0.436332313-0.197222205;
+ break;
+ }
+ if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1
+ setpoint2 = angle;
+ count = 0;
+ state=2;
+ }
+ /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) {
+ state = 2;
+ }*/
+ break;
}
- /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) {
- state = 2;
- }*/
- break;
-
- case 2:
- setpoint1 = 0;
- count++;
- if(count>1000) {
- count = 0;
- state = 3;
+ case 2: {
+ setpoint1 = 0;
+ count++;
+ if(count>1000) {
+ count = 0;
+ state = 3;
+ }
+ break;
}
- break;
- case 3:
- switch (force) {
- case 1:
- setpoint1 += 2.5*TSAMP; //6.8*TSAMP;
- break;
- case 2:
- setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
- break;
- case 3:
- setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
- break;
+ case 3: {
+ switch (force) {
+ case 1:
+ setpoint1 += 2*TSAMP; //6.8*TSAMP;
+ break;
+ case 2:
+ setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
+ break;
+ case 3:
+ setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
+ break;
+ }
+ if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
+ state = 4;
+ }
+ break;
}
- if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
- state = 4;
- }
- break;
- case 4:
- setpoint2 -= 0.25*TSAMP;
- 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.5*TSAMP;
- if(setpoint1 < 0) {
- state = 6;
+ case 4: {
+ setpoint2 -= 0.25*TSAMP;
+ if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm
+ state = 5;
+ }
+ break;
}
- break;
- case 6:
- setpoint1 = 0;
- count++;
- if(count>3000) {
- count = 0;
- state = 1;
- goto directionchoice;
+ case 5: {
+ setpoint1 -= 0.5*TSAMP;
+ if(setpoint1 < 0) {
+ state = 6;
+ }
+ break;
}
- break;
- }
-
- //motor regeling
+ case 6: {
+ setpoint1 = 0;
+ count++;
+ if(count>3000) {
+ count = 0;
+ state = 1;
+ goto directionchoice;
+ }
+ break;
+ }
+ }
- //regelaar motor1, bepaalt positie
- controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
- integral1 = integral1 + controlerror1*TSAMP;
- derivative1 = (controlerror1 - previouserror1)/TSAMP;
- pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
- previouserror1 = controlerror1;
+ //motor regeling
- keep_in_range(&pwm1, -1,1);
- pwm_motor1.write(fabs(pwm1));
- if(pwm1 > 0) {
- motor1dir = 1;
- } else {
- motor1dir = 0;
- }
+ //regelaar motor1, bepaalt positie
+ controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
+ integral1 = integral1 + controlerror1*TSAMP;
+ derivative1 = (controlerror1 - previouserror1)/TSAMP;
+ pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
+ previouserror1 = controlerror1;
+
+ keep_in_range(&pwm1, -1,1);
+ pwm_motor1.write(fabs(pwm1));
+ if(pwm1 > 0) {
+ motor1dir = 1;
+ } else {
+ motor1dir = 0;
+ }
- //regelaar motor2, bepaalt positie
- controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
- integral = integral + controlerror*TSAMP;
- derivative = (controlerror - previouserror)/TSAMP;
- pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
- previouserror = controlerror;
+ //regelaar motor2, bepaalt positie
+ controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
+ integral = integral + controlerror*TSAMP;
+ derivative = (controlerror - previouserror)/TSAMP;
+ pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
+ previouserror = controlerror;
+
+ keep_in_range(&pwm, -1,1);
+ pwm_motor2.write(fabs(pwm));
+ if(pwm > 0) {
+ motor2dir = 1;
+ } else {
+ motor2dir = 0;
+ }
+
+ /*
+ //controleert of batje positie heeft bepaald
+ if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+ if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
+ } else {
+ pwm_motor2.write(0);
+ batjeset = integral = derivative = previouserror = 0;
+ wait(1);
+ //goto motor1control;
+ }
+ */
+ }
+ /*
+ motor1control:
+ while(1) { // loop voor het slaan mbv motor1 (batje snelheid)
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ if (balhit == 0) { //regelaar motor1, bepaalt snelheid
+ controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+ integral = integral + controlerror*TSAMP;
+ derivative = (controlerror - previouserror)/TSAMP;
+ pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
+ previouserror = controlerror;
+ } else { //regelaar motor1, bepaalt positie
+ balhit = integral = derivative = previouserror = 0;
+ goto resetpositionmotor1;
+ }
+
+ keep_in_range(&pwm, -1,1);
+ pwm_motor1.write(abs(pwm));
+
+ if(pwm > 0) {
+ motor1dir = 1;
+ } else {
+ motor1dir = 0;
+ }
- keep_in_range(&pwm, -1,1);
- pwm_motor2.write(fabs(pwm));
- if(pwm > 0) {
- motor2dir = 1;
- } else {
- motor2dir = 0;
- }
+ //controleert of batje balletje heeft bereikt
+ //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
+ if (motor1.getPosition()*omrekenfactor1 > 1.60) {
+ balhit = 1;
+ }
+ }
+ // FORMAT_CODE_END
+
+ resetpositionmotor1:
+ while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ //regelaar motor1, bepaalt positie
+ controlerror = -1*motor1.getPosition()*omrekenfactor1;
+ integral = integral + controlerror*TSAMP;
+ derivative = (controlerror - previouserror)/TSAMP;
+ pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
+ previouserror = controlerror;
+
+ keep_in_range(&pwm, -1,1);
+ if(pwm > 0) {
+ motor1dir = 1;
+ } else {
+ motor1dir = 0; //1 = rechtsom, 0 = linksom
+ }
+
+ pwm_motor1.write(abs(pwm));
- /*
+ //controleert of arm terug in positie is
+ if(batjeset < 200) {
+ if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
+ } else {
+ pwm_motor1.write(0);
+ batjeset = integral = derivative = previouserror = 0;
+ wait(1);
+ goto resetpositionmotor2;
+ }
+ }
+
+ resetpositionmotor2:
+ while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
+ while(!looptimerflag);
+ looptimerflag = false; //clear flag
+
+ //regelaar motor2, bepaalt positie
+ controlerror = -1*motor2.getPosition()*omrekenfactor2;
+ integral = integral + controlerror*TSAMP;
+ derivative = (controlerror - previouserror)/TSAMP;
+ pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
+ previouserror = controlerror;
+
+ keep_in_range(&pwm, -1,1);
+
+ if(pwm > 0) {
+ motor2dir = 1;
+ } else {
+ motor2dir = 0;
+ }
+
+ pwm_motor2.write(abs(pwm));
+
//controleert of batje positie heeft bepaald
if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
- if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
+ if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
batjeset = 0;
} else {
batjeset++;
@@ -657,115 +779,8 @@
pwm_motor2.write(0);
batjeset = integral = derivative = previouserror = 0;
wait(1);
- //goto motor1control;
+ direction = force = 0;
+ goto motor1cal;
}
- */
- }
- /*
- motor1control:
- while(1) { // loop voor het slaan mbv motor1 (batje snelheid)
- while(!looptimerflag);
- looptimerflag = false; //clear flag
-
- if (balhit == 0) { //regelaar motor1, bepaalt snelheid
- controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
- integral = integral + controlerror*TSAMP;
- derivative = (controlerror - previouserror)/TSAMP;
- pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
- previouserror = controlerror;
- } else { //regelaar motor1, bepaalt positie
- balhit = integral = derivative = previouserror = 0;
- goto resetpositionmotor1;
- }
-
- keep_in_range(&pwm, -1,1);
- pwm_motor1.write(abs(pwm));
-
- if(pwm > 0) {
- motor1dir = 1;
- } else {
- motor1dir = 0;
- }
-
- //controleert of batje balletje heeft bereikt
- //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
- if (motor1.getPosition()*omrekenfactor1 > 1.60) {
- balhit = 1;
- }
- }
- // FORMAT_CODE_END
-
- resetpositionmotor1:
- while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
- while(!looptimerflag);
- looptimerflag = false; //clear flag
-
- //regelaar motor1, bepaalt positie
- controlerror = -1*motor1.getPosition()*omrekenfactor1;
- integral = integral + controlerror*TSAMP;
- derivative = (controlerror - previouserror)/TSAMP;
- pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
- previouserror = controlerror;
-
- keep_in_range(&pwm, -1,1);
- if(pwm > 0) {
- motor1dir = 1;
- } else {
- motor1dir = 0; //1 = rechtsom, 0 = linksom
- }
-
- pwm_motor1.write(abs(pwm));
-
- //controleert of arm terug in positie is
- if(batjeset < 200) {
- if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
- batjeset = 0;
- } else {
- batjeset++;
- }
- } else {
- pwm_motor1.write(0);
- batjeset = integral = derivative = previouserror = 0;
- wait(1);
- goto resetpositionmotor2;
- }
- }
-
- resetpositionmotor2:
- while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
- while(!looptimerflag);
- looptimerflag = false; //clear flag
-
- //regelaar motor2, bepaalt positie
- controlerror = -1*motor2.getPosition()*omrekenfactor2;
- integral = integral + controlerror*TSAMP;
- derivative = (controlerror - previouserror)/TSAMP;
- pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
- previouserror = controlerror;
-
- keep_in_range(&pwm, -1,1);
-
- if(pwm > 0) {
- motor2dir = 1;
- } else {
- motor2dir = 0;
- }
-
- pwm_motor2.write(abs(pwm));
-
- //controleert of batje positie heeft bepaald
- if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
- if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
- batjeset = 0;
- } else {
- batjeset++;
- }
- } else {
- pwm_motor2.write(0);
- batjeset = integral = derivative = previouserror = 0;
- wait(1);
- direction = force = 0;
- goto motor1cal;
- }
- }*/
-} // end main
\ No newline at end of file
+ }*/
+ } // end main
\ No newline at end of file