Diff: main.cpp

Revision:

20:d40b6cba4280

Parent:

19:1bd2fc3bce1e

Child:

21:674fafb6301d

--- a/main.cpp	Wed Oct 29 08:59:07 2014 +0000
+++ b/main.cpp	Wed Oct 29 15:08:30 2014 +0000
@@ -2,18 +2,57 @@
 #include "HIDScope.h"
 #include "arm_math.h"
 #include "MODSERIAL.h"
+#include "encoder.h"
 
-Serial pc(USBTX, USBRX); // tx, rx
+#define TSAMP 0.01
+#define K_P (0.1)
+#define K_I (0.03  *TSAMP)
+#define K_D (0.001 /TSAMP)
+#define I_LIMIT 1.
+
+#define M1_PWM PTC8
+#define M1_DIR PTC9
+#define M2_PWM PTA5
+#define M2_DIR PTA4
+
+//Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
+
+Serial pc(USBTX, USBRX);
+
 DigitalOut myled1(LED_RED);
 DigitalOut myled2(LED_GREEN);
 DigitalOut myled3(LED_BLUE);
-PwmOut     motorsignal(PTD4);
 
 //Define objects
 AnalogIn    emg0(PTB1); //Analog input
 AnalogIn    emg1(PTB2); //Analog input
 HIDScope scope(2);
 
+//motor 25D
+Encoder motor1(PTD3,PTD5,true); //wit, geel
+PwmOut pwm_motor1(M2_PWM);
+DigitalOut motordir1(M2_DIR);
+
+//motor2 37D
+Encoder motor2(PTD2, PTD0,true); //wit, geel
+PwmOut pwm_motor2(M1_PWM);
+DigitalOut motordir2(M1_DIR);
+
+float speed1;
+float hoek1;
+float speed2;
+float hoek2;
+
+bool flip=false;
+
+void attime()
+{
+    flip = !flip;
+}
+
+
+// EMG
+
 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
 //lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB
@@ -211,6 +250,90 @@
     myled3 = 1;
 }
 
+// MOTORFUNCTIES
+
+void motor2_speed_low ()
+{
+    wait(1);
+    speed2 = 1;
+    motordir2=1;
+    pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
+    wait(0.3);             //naar 140 graden
+    pwm_motor2.write(0);    //CCW
+    wait(1);
+    motordir2=0;
+    pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
+    wait(0.20);             //balletje slaan, 160 graden
+    pwm_motor2.write(0);
+    wait(1);
+    motordir2=1;            //CW
+    pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
+    wait(1);            //terug naar begin positie, 20 graden
+    pwm_motor2.write(0);
+}
+
+void motor2_speed_mid ()
+{
+    wait(1);
+    speed2 = 1;
+    motordir2=1;
+    pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
+    wait(0.3);             //naar 140 graden
+    pwm_motor2.write(0);    //CCW
+    wait(1);
+    motordir2=0;
+    pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
+    wait(0.20);             //balletje slaan, 160 graden
+    pwm_motor2.write(0);
+    wait(1);
+    motordir2=1;            //CW
+    pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
+    wait(1);            //terug naar begin positie, 20 graden
+    pwm_motor2.write(0);
+}
+
+void motor2_speed_high ()
+{
+    wait(1);
+    speed2 = 1;
+    motordir2=1;
+    pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
+    wait(0.3);             //naar 140 graden
+    pwm_motor2.write(0);    //CCW
+    wait(1);
+    motordir2=0;
+    pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
+    wait(0.20);             //balletje slaan, 160 graden
+    pwm_motor2.write(0);
+    wait(1);
+    motordir2=1;            //CW
+    pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
+    wait(1);            //terug naar begin positie, 20 graden
+    pwm_motor2.write(0);
+}
+
+void motor1_links()
+{
+    speed1 = 0.7;
+    hoek1 = 0.09; //in seconde
+    wait(1);
+    motordir1=0;            //aangeven van directie (0 = CCW)
+    pwm_motor1.write(speed1);  //snelheid van de motor
+    wait(hoek1);             //Hierdoor kun je het aantal graden bepalen die de as draait
+    pwm_motor1.write(0);
+}
+
+
+void motor1_rechts()
+{
+    speed1 = 0.7;
+    hoek1 = 0.09; //in seconde
+    wait(1);
+    motordir1=1;            //aangeven van directie (1 = CW)
+    pwm_motor1.write(speed1);  //snelheid van de motor
+    wait(hoek1);             //Hierdoor kun je het aantal graden bepalen die de as draait
+    pwm_motor1.write(0);
+}
 
 int main()
 {
@@ -232,6 +355,7 @@
         /*Everything is handled by the interrupt routine now!*/
 
         while(1) {
+            static float count = 0;
             pc.printf("Span de biceps aan om het instellen te starten.\n");
             do {
                 ShineRed();
@@ -241,10 +365,10 @@
                 BlinkGreen();
                 while (1) {
                     pc.printf("In de loop.\n");
-                    if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) {
+                    if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
                         stopblinkgreen();
-                        pc.printf("Shinered.\n");
-                        ShineRed();
+                        pc.printf("ShineGreen.\n");
+                        ShineGreen();
                         wait (4);
                         break;
                     }
@@ -252,14 +376,16 @@
                         stopblinkgreen();
                         pc.printf("ShineBlue.\n");
                         ShineBlue();
+                        motor1_links();
                         wait(4);
                         break;
-                    } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05)
+                    } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
 
                     {
                         stopblinkgreen();
-                        pc.printf("ShineGreen.\n");
-                        ShineGreen();
+                        pc.printf("ShineRed.\n");
+                        ShineRed();
+                        motor1_rechts();
                         wait (4);
                         break;
                     }
@@ -267,10 +393,11 @@
                 BlinkGreen();
                 while (1) {
                     pc.printf("In de loop.\n");
-                    if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) {
+                    if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
                         stopblinkgreen();
-                        pc.printf("Shinered.\n");
-                        ShineRed();
+                        pc.printf("ShineGreen.\n");
+                        ShineGreen();
+                        motor2_speed_mid ();
                         wait (4);
                         break;
                     }
@@ -278,14 +405,16 @@
                         stopblinkgreen();
                         pc.printf("ShineBlue.\n");
                         ShineBlue();
+                        motor2_speed_low ();
                         wait(4);
                         break;
-                    } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05)
+                    } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
 
                     {
                         stopblinkgreen();
-                        pc.printf("ShineGreen.\n");
-                        ShineGreen();
+                        pc.printf("ShineRed.\n");
+                        ShineRed();
+                        motor2_speed_high ();
                         wait (4);
                         break;
                     }