Werkcollege opgave 23 september BMT K9
Dependencies:
Encoder
HIDScope
MODSERIAL
mbed
QEI
biquadFilter
Revision 52:45200bbde108, committed 2015-10-28
- Comitter:
- ThomasBNL
- Date:
- Wed Oct 28 18:57:19 2015 +0000
- Parent:
- 51:b40967b1fe5c
- Commit message:
- Programma Step response HIT scope Position (drie seconde full speed ene kant rust drie seconde full speed andere kant) -> kijk uit dat de snoeren niet in de war raken
Changed in this revision
diff -r b40967b1fe5c -r 45200bbde108 main.cpp
--- a/main.cpp Mon Oct 26 16:06:26 2015 +0000
+++ b/main.cpp Wed Oct 28 18:57:19 2015 +0000
@@ -30,7 +30,7 @@
// [DEFINE CONSTANTS] //
-HIDScope scope(3); // Aantal HIDScope kanalen
+HIDScope scope(4); // Aantal HIDScope kanalen
MODSERIAL pc(USBTX,USBRX);
Ticker control_tick;
Ticker T1;
@@ -123,8 +123,9 @@
void keep_in_range (double * in, double min, double max);
-double pwm_strike;
double f; double pwm_average; double smp;
+double position_strike, position_turn, pwm_strike, pwm_turn;
+double conversion_counts_to_degrees=0.085877862594198;
//==========================//
// [MAIN FUNCTION] //
//==========================//
@@ -133,62 +134,113 @@
control_tick.attach(&ControlGo, (float)1/Fs);
pc.baud(115200);
//double n=0;
- calibration(); // calibreer min en max positie
+ //calibration(); // calibreer min en max positie
f=1;
smp=0;
while(1)
{
- // if(control_go)
-// {
-// //n++;
-// //pc.printf("n %f",n);
-// sample_filter();
-// scope.set(0,EMG_left_Bicep); //left bicep unfiltered
-// scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
-// scope.set(2,moving_average_left); //
-// scope.send();
-// control_go = 0;
-// }
+ if(f==1)
+ {
+ pwm_motor_strike=0;
+ pwm_motor_turn=0;
+ pc.printf("start /n");
+ }
+ if(f==512)
+ {
+ pwm_motor_strike=1;
+ motordirection_strike=1;
+ pwm_motor_turn=1;
+ motordirection_turn=1;
+ }
+
+ if(f==2048)
+ {
+ pc.printf("stop /n");
+ pwm_motor_strike=0;
+ pwm_motor_turn=0;
+ }
+
+ if(f==2560) // same cycle but referesed direction
+ {
+ pwm_motor_strike=0;
+ pwm_motor_turn=0;
+ pc.printf("start /n");
+ }
+ if(f==3072)
+ {
+ pwm_motor_strike=1;
+ motordirection_strike=0;
+ pwm_motor_turn=1;
+ motordirection_turn=0;
+ }
+
+ if(f==4500)
+ {
+ pc.printf("stop /n");
+ pwm_motor_strike=0;
+ pwm_motor_turn=0;
+ f=0;
+ }
+
yellow();
if(control_go)
- {
+ {
control_go=0;
- sample_filter();
- scope.set(0,EMG_left_Bicep); //left bicep unfiltered
- scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
- scope.set(2,moving_average_left); //
+ f++;
+
+ if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value exceeds -4200 and 4200 (number of counts equal to one revolution) than reset to zero
+ { motor_turn.reset(); }
+
+ position_turn = conversion_counts_to_degrees * motor_turn.getPulses(); // Convert counts to degrees
+ pwm_strike=pwm_motor_strike;
+ pwm_turn=pwm_motor_turn;
+
+ if ((motor_strike.getPulses()>4200) || (motor_strike.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
+ { motor_strike.reset(); }
+
+ position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
+
+ scope.set(0,pwm_strike); //ik weet niet of dit weergegeven kan worden
+ scope.set(1,pwm_turn); //Filtered signal
+ scope.set(2,position_turn); //
+ scope.set(3,position_strike); //
scope.send();
- double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
- double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
- pwm_strike=signal_above_threshold/max_signal;
- keep_in_range(&pwm_strike, 0,1);
- pwm_strike=pwm_strike*pwm_strike;
-
- pwm_average=pwm_strike+pwm_average/f;
- f++;
- smp++;
- pc.printf("Pwm=%f \n\r", pwm_average);
- double speed=fabs(pwm_average);
+ }
-if (pwm_average < 0.1) { leduit(); }
-if (pwm_average > 0.1) { ledgreen1 = ledon; }
-if (pwm_average > 0.2) { ledgreen2 = ledon; }
-if (pwm_average > 0.3) { ledyellow1 = ledon; }
-if (pwm_average > 0.5) { ledyellow2 = ledon; }
-if (pwm_average > 0.7) { ledred1 = ledon; }
-if (pwm_average > 0.9) { ledred2 = ledon; }
+
+
+// double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
+// double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
+// pwm_strike=signal_above_threshold/max_signal;
+// keep_in_range(&pwm_strike, 0,1);
+// pwm_strike=pwm_strike*pwm_strike;
+//
+// pwm_average=pwm_strike+pwm_average/f;
+// f++;
+// smp++;
+// pc.printf("Pwm=%f \n\r", pwm_average);
+// double speed=fabs(pwm_average);
+//
+//
+//if (pwm_average < 0.1) { leduit(); }
+//if (pwm_average > 0.1) { ledgreen1 = ledon; }
+//if (pwm_average > 0.2) { ledgreen2 = ledon; }
+//if (pwm_average > 0.3) { ledyellow1 = ledon; }
+//if (pwm_average > 0.5) { ledyellow2 = ledon; }
+//if (pwm_average > 0.7) { ledred1 = ledon; }
+//if (pwm_average > 0.9) { ledred2 = ledon; }
- if(smp>512)
- {
- pwm_motor_strike=speed;
- pwm_motor_turn=speed;
- green();
- pwm_motor_strike=0;
- wait(3);
- f=1;
- smp=0;}
- }
+ // if(smp>512)
+// {
+// pwm_motor_strike=speed;
+// pwm_motor_turn=speed;
+// green();
+// pwm_motor_strike=0;
+// wait(3);
+// f=1;
+// smp=0;}
+// }
} // while end
} // main end
Bouke Scheltinga
