Jesse Kaiser
4 directional EMG control of the XY table. Made during my bachelor end assignment.
Dependencies: C12832_lcd HIDScope mbed-dsp mbed
Revision 61:99f3ae6e053b, committed 2015-06-17
- Comitter:
- jessekaiser
- Date:
- Wed Jun 17 10:09:51 2015 +0000
- Parent:
- 60:664f9b907c02
- Child:
- 62:509622cce1c6
- Commit message:
- Testen van homing;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Jun 16 15:42:59 2015 +0000
+++ b/main.cpp Wed Jun 17 10:09:51 2015 +0000
@@ -24,6 +24,9 @@
#define EMG_tresh2 0.02
#define EMG_tresh3 0.01
#define EMG_tresh4 0.01
+#define H_Gain 5
+#define Pt_x 0.83
+#define Pt_y 0.25
//Motor control
DigitalOut Dirx(p21);
@@ -96,6 +99,12 @@
float speed1, speed2, speed3, speed4;
float damping1, damping2, damping3, damping4;
float emg_x, emg_y;
+float cx = 0;
+float cy = 0;
+float errorx = 0.2;
+float errory = 0.2;
+float Ps_x = 0;
+float Ps_y = 0;
void looper_emg()
{
@@ -200,77 +209,74 @@
int main()
{
- float errorx = 0.2;
- float errory = 0.2;
- float Ps_x = Posx.read();
- float Ps_y = Posy.read();
- float cx = 0;
- float cy = 0;
- #define H_Gain 1
- #define Pt_x 0.83
- #define Pt_y 0.25
-
// Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
// scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
- while(errorx > 0.05 || errory > 0.05) {
- lcd.printf("%.2f %.2f \n", Stepx.read(), Stepy.read());
- if (Posx.read() < 0.83) {
- Dirx = 0;
- errorx = Pt_x - Ps_x;
- cx = errorx * H_Gain;
- //Stepx.period(1.0/(2000*cx));
- }
- if (Posy.read() > 0.25) {
- Diry = 0;
- errory = Ps_y - Pt_y;
- cy = errory * H_Gain;
- //Stepy.period(1.0/(2000*cy));
- }
-
- Stepx.period(1.0/(2000*cx));
- Stepy.period(1.0/(2000*cy));
- wait(0.01);
-
- }
-
- Ticker emgtimer; //biceps
- arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
- arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
- //triceps
- arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1 , lowpass_const, lowpass_triceps_states);
- arm_biquad_cascade_df1_init_f32(&highnotch_triceps, 2 , highnotch_const, highnotch_triceps_states);
- //pectoralis major
- arm_biquad_cascade_df1_init_f32(&lowpass_pect, 1 , lowpass_const, lowpass_pect_states);
- arm_biquad_cascade_df1_init_f32(&highnotch_pect, 2 , highnotch_const, highnotch_pect_states);
- //deltoid
- arm_biquad_cascade_df1_init_f32(&lowpass_deltoid, 1 , lowpass_const, lowpass_deltoid_states);
- arm_biquad_cascade_df1_init_f32(&highnotch_deltoid, 2 , highnotch_const, highnotch_deltoid_states);
- emgtimer.attach(looper_emg, 0.01);
-
- Ticker looptimer1;
- looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
-
- Ticker looptimer2;
- looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
-
- //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
MS1 = 1;
MS2 = 0;
MS3 = 0;
Stepx.write(0.5); // Duty cycle of 50%
Stepy.write(0.5);
+ while(errorx > 0.05 || errory > 0.05) {
+ lcd.printf("%.2f %.2f \n", errorx, errory);
+ Ps_x = Posx.read();
+ Ps_y = Posy.read();
+ if (Ps_x < 0.83) {
+ Dirx = 0;
+ errorx = Pt_x - Ps_x;
+ cx = errorx * H_Gain;
+ Stepx.period(1.0/(setpoint*cx));
+ }
+ if (Ps_y > 0.25) {
+ Diry = 0;
+ errory = Ps_y - Pt_y;
+ cy = errory * H_Gain;
+ Stepy.period(1.0/(setpoint*cy));
+ }
+ wait(0.01);
+ }
+
+ lcd.printf("Done");
+ Enablex = 1;
+ Enabley = 1;
+ /* Ticker emgtimer; //biceps
+ arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
+ arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
+ //triceps
+ arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1 , lowpass_const, lowpass_triceps_states);
+ arm_biquad_cascade_df1_init_f32(&highnotch_triceps, 2 , highnotch_const, highnotch_triceps_states);
+ //pectoralis major
+ arm_biquad_cascade_df1_init_f32(&lowpass_pect, 1 , lowpass_const, lowpass_pect_states);
+ arm_biquad_cascade_df1_init_f32(&highnotch_pect, 2 , highnotch_const, highnotch_pect_states);
+ //deltoid
+ arm_biquad_cascade_df1_init_f32(&lowpass_deltoid, 1 , lowpass_const, lowpass_deltoid_states);
+ arm_biquad_cascade_df1_init_f32(&highnotch_deltoid, 2 , highnotch_const, highnotch_deltoid_states);
+ emgtimer.attach(looper_emg, 0.01);
- while (1) {
+ Ticker looptimer1;
+ looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
+
+ Ticker looptimer2;
+ looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
+
+ //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
+ MS1 = 1;
+ MS2 = 0;
+ MS3 = 0;
+ Stepx.write(0.5); // Duty cycle of 50%
+ Stepy.write(0.5);
- //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read());
- //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values
- //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor
- lcd.printf("%.2f %.2f \n", force1, force2);
- //lcd.printf("%.2f, %.2f %.2f %.2f \n", gain_biceps, gain_triceps, gain_pect, gain_deltoid);
- //lcd.printf("%.2f, %.2f %.2f %.2f \n", norm_biceps, norm_triceps, norm_pect, norm_deltoid);
- wait(0.01);
+ while (1) {
+
- }
+ //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read());
+ //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values
+ //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor
+ lcd.printf("%.2f %.2f \n", force1, force2);
+ //lcd.printf("%.2f, %.2f %.2f %.2f \n", gain_biceps, gain_triceps, gain_pect, gain_deltoid);
+ //lcd.printf("%.2f, %.2f %.2f %.2f \n", norm_biceps, norm_triceps, norm_pect, norm_deltoid);
+ wait(0.01);
+
+ }*/
}