Revision 64:5a66361c8318, committed 2015-06-18

Comitter:

jessekaiser

Date:

Thu Jun 18 11:19:10 2015 +0000

Parent:

63:e336c16c8957

Child:

65:821683c7eb98

Commit message:

Homing 3

Changed in this revision

--- a/main.cpp	Wed Jun 17 15:05:25 2015 +0000
+++ b/main.cpp	Thu Jun 18 11:19:10 2015 +0000
@@ -26,8 +26,8 @@
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
 #define H_Gain  2
-#define Pt_x    0.88
-#define Pt_y    0.25
+#define Pt_x    44
+#define Pt_y    10
 
 //Motor control
 DigitalOut Dirx(p21);
@@ -224,92 +224,97 @@
     Stepy.write(0.5);
     //Stepx.period(1/1500);
     //wait(0.01);
-    //Stepy.period(1/1500); 
+    //Stepy.period(1/1500);
     //wait(0.01);
     //}
     Enablex = 1;
     Enabley = 1;
-    wait(1); 
+    wait(1);
     lcd.printf("Start homing");
     wait(2);
     lcd.cls();
     wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > 0.03 || errory > 0.03) {
+    while(errorx > 2 && errory > 2) {
         lcd.printf("%.2f %.2f  \n", Ps_x, Ps_y);
 
-        Ps_x = Posx.read();
-        Ps_y = Posy.read();
+        Ps_x = Posx.read()* 50;
+        Ps_y = Posy.read()* 40;
 
-        if (Ps_x < 0.88) {
+        if (Ps_x < 44) {
             Dirx = 0;
             errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
-            
+
             float hnew_step_freqx;
             hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
             hstep_freqx = hnew_step_freqx;
             Stepx.period(1.0/hstep_freqx);
             wait(0.01);
+        } else {
+            Enablex = 1;
         }
-        if (Ps_y > 0.25) {
+
+        if (Ps_y > 10) {
             Diry = 0;
             errory = Ps_y - Pt_y;
             cy = errory * H_Gain;
-            
+
             float hnew_step_freqy;
             hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
             hstep_freqy = hnew_step_freqy;
             Stepy.period(1.0/hstep_freqy);
             wait(0.01);
+        } else {
+            Enabley = 1;
         }
-     
+
     }
     lcd.cls();
     wait(1);
     lcd.printf("Done");
     Enablex = 1;
     Enabley = 1;
-    wait(3); 
-   /*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);
+    wait(3);
+    /*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 looptimer1;
+      looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
 
-     Ticker looptimer2;
-     looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
+      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);
+      //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 (1) {
+      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);
+          //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);
 
-     }*/
+      }*/
 }