Diff: main.cpp

Revision:

79:251d73ddbc8b

Parent:

78:9cae6de48b0e

Child:

80:6f9ddb8bb335

--- a/main.cpp	Mon Jun 22 11:52:27 2015 +0000
+++ b/main.cpp	Mon Jun 22 13:13:55 2015 +0000
@@ -8,7 +8,7 @@
 */
 
 #include "mbed.h"
-#include "C12832_lcd.h"
+//#include "C12832_lcd.h"
 #include "arm_math.h"
 //#include "HIDScope.h"
 
@@ -22,41 +22,41 @@
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
 #define H_Gain  3.5
-#define Pt_x    0.50
-#define Pt_y    0.50
+#define Pt_x    0.83
+#define Pt_y    0.25
 #define error_tresh 0.01
 
 //Motor control
-DigitalOut Dirx(p21);
-PwmOut Stepx(p22);
-DigitalOut Diry(p23);
-PwmOut Stepy(p24);
+DigitalOut Dirx(PB_8);
+PwmOut Stepx(PB_9);
+DigitalOut Diry(PA_2);
+PwmOut Stepy(PA_3);
 
 //Signal to and from computer
 Serial pc(USBTX, USBRX);
 
 //Position sensors
-AnalogIn Posx(p19);
-AnalogIn Posy(p20);
-DigitalOut Enablex(p25);
-DigitalOut Enabley(p26);
+AnalogIn Posx(PC_0);
+AnalogIn Posy(PC_1);
+DigitalOut Enablex(PB_3);
+DigitalOut Enabley(PA_10);
 
 //Microstepping
-DigitalOut MS1(p27);
-DigitalOut MS2(p28);
-DigitalOut MS3(p29);
+DigitalOut MS1(PB_10);
+DigitalOut MS2(PB_4);
+DigitalOut MS3(PB_5);
 
 //EMG inputs
-AnalogIn emg1(p15); 
-AnalogIn emg2(p16); 
-AnalogIn emg3(p17);
-AnalogIn emg4(p18);
+AnalogIn emg1(PB_0); 
+AnalogIn emg2(PA_4); 
+AnalogIn emg3(PA_1);
+AnalogIn emg4(PA_0);
 
 //HIDScope scope(4);
 //Ticker   scopeTimer;
 
 //lcd screen
-C12832_LCD lcd;
+//C12832_LCD lcd;
 
 //Variables for motor control
 float setpoint = 2000; //Frequentie setpoint
@@ -90,7 +90,7 @@
 
 //global variabels
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
-float speed_old1, speed_old;
+float speed_old1, speed_old2;
 float acc1, acc2;
 float force1, force2;
 float speed1, speed2;
@@ -177,7 +177,7 @@
 }
 
 
-/*void looper_motorx()
+void looper_motorx()
 {
 
     emg_x = (filtered_pect - filtered_deltoid);
@@ -209,7 +209,7 @@
         Enablex = 0;
     }
 
-}*/
+}
 
 int main()
 {
@@ -226,9 +226,9 @@
         Enablex = 1;
         Enabley = 1;
         wait(1);
-        lcd.printf("Start homing");
+        pc.printf("Start homing");
         wait(2);
-        lcd.cls();
+        //lcd.cls();
         wait(1);
         Enablex = 0;
         Enabley = 0;
@@ -238,19 +238,10 @@
             Ps_y = Posy.read();
             errorx = fabs(Pt_x - Ps_x);
             errory = fabs(Ps_y - Pt_y);
-            lcd.printf("%.2f %.2f \n", errorx, errory);
+            pc.printf("%.2f %.2f \n", errorx, errory);
 
 
-            if (Ps_x < 0.50 && errorx > error_tresh) {
-                Dirx = 0;
-                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);
-            }
-            if (Ps_y > 0.50 && errory > error_tresh) {
+            if (Ps_y > Pt_y && errory > error_tresh) {
                 Diry = 0;
                 cy = errory * H_Gain;
                 float hnew_step_freqy;
@@ -260,16 +251,7 @@
                 wait(0.01);
             }
 
-            if (Ps_x > 0.50 && errorx > error_tresh) {
-                Dirx = 1;
-                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);
-            }
-            if (Ps_y < 0.50 && errory > error_tresh) {
+            if (Ps_y < Pt_y && errory > error_tresh) {
                 Diry = 1;
                 cy = errory * H_Gain;
                 float hnew_step_freqy;
@@ -280,16 +262,16 @@
             }
 
         }
-        lcd.printf("Done");
+        pc.printf("Done");
         wait(2);
-        lcd.cls();
+        //lcd.cls();
         wait(1);
         Enablex = 1;
         Enabley = 1;
         wait(3);
-        lcd.printf("Start EMG Control");
+        pc.printf("Start EMG Control");
         wait(2);
-        lcd.cls();
+        //lcd.cls();
         wait(1);
         Enablex = 0;
         Enabley = 0;
@@ -314,8 +296,8 @@
     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