Diff: main.cpp

Revision:

22:f3a827faa135

Parent:

21:b7fb79882cb8

Child:

23:8f7ce4894c58

--- a/main.cpp	Fri Oct 31 13:30:21 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:59:31 2014 +0000
@@ -108,7 +108,7 @@
 standen hoek2 = STAND1;
 
 int main ()
-{ 
+{
     pc.baud(115200);
 
     drempelwaardeT=0;
@@ -239,169 +239,169 @@
                 myled3 = 0;
             } else {*/
 
-                /*pc.printf("eerst positie dan snelheid aangeven /n");
+            /*pc.printf("eerst positie dan snelheid aangeven /n");
 
-                //bepaling van positie met triceps 1
-                Ticker log_timerT1;
+            //bepaling van positie met triceps 1
+            Ticker log_timerT1;
+
+            arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
+            arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+            arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
 
-                arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
-                arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
-                arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+            myled1 = 0;
+            myled2 = 1;
+            myled3 = 1;
+
+            log_timerT1.attach(Triceps, 0.005);
+            wait(2);
+            log_timerT1.detach();
 
-                myled1 = 0;
-                myled2 = 1;
-                myled3 = 1;
+            // positie van batje met behulp van Triceps
 
-                log_timerT1.attach(Triceps, 0.005);
-                wait(2);
-                log_timerT1.detach();
+            if (MOVAVG_T >= drempelwaardeT) {
+                yT1=1;
+            } else {
+                yT1=0;
+            }
 
-                // positie van batje met behulp van Triceps
+            pc.printf("Triceps meting 1 is klaar.\n");
+            myled1 = 1;
+            myled2 = 1;
+            myled3 = 0;
+            wait(3);
 
-                if (MOVAVG_T >= drempelwaardeT) {
-                    yT1=1;
-                } else {
-                    yT1=0;
-                }
+            //bepaling van positie met tricep 2
+            Ticker log_timerT2;
+
+            arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
+            arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+            arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+
+            myled1 = 0;
+            myled2 = 1;
+            myled3 = 1;
 
-                pc.printf("Triceps meting 1 is klaar.\n");
-                myled1 = 1;
-                myled2 = 1;
-                myled3 = 0;
-                wait(3);
+            log_timerT2.attach(Triceps, 0.005);
+            wait(2);
+            log_timerT2.detach();
 
-                //bepaling van positie met tricep 2
-                Ticker log_timerT2;
+            if (MOVAVG_T >= drempelwaardeT) {
+                yT2=1;
+            } else {
+                yT2=0;
+            }
+
+            pc.printf("Triceps meting 2 is klaar.\n");
+            myled1 = 1;
+            myled2 = 1;
+            myled3 = 0;
+
+            //*** INPUT MOTOR 2 ***
+            positie=yT1+yT2;
 
-                arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
-                arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
-                arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
-
-                myled1 = 0;
-                myled2 = 1;
-                myled3 = 1;
+            //controle positie op scherm
+            if (positie==0) {
+                pc.printf("Motor 2 blijft op stand 1\n");
+            } else {
+                if (positie==1) {
+                    pc.printf("Motor 2 gaat naar stand 2\n");
+                } else {
+                    if (positie==2) {
+                        pc.printf("Motor 2 gaat naar stand 3\n");
+                    }
+                }
+            }
 
-                log_timerT2.attach(Triceps, 0.005);
-                wait(2);
-                log_timerT2.detach();
+            Ticker looptimer2;
+            looptimer2.attach(motor2aansturing,TSAMP1);
+            wait(8);
+            looptimer2.detach();
+            pc.printf("Detach Motor 1\n"); */
+
+//------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
+            wait(2);
+            /* Ticker log_timerB;
 
-                if (MOVAVG_T >= drempelwaardeT) {
-                    yT2=1;
-                } else {
-                    yT2=0;
-                }
+            arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
+            arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
+            arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
+
+            myled1 = 1;
+            myled2 = 0;
+            myled3 = 1;
+
+            log_timerB.attach(Biceps,0.005);
+            wait(2);
+            log_timerB.detach();
+
+            //bepaling van snelheidsstand met biceps
 
-                pc.printf("Triceps meting 2 is klaar.\n");
-                myled1 = 1;
-                myled2 = 1;
-                myled3 = 0;
-
-                //*** INPUT MOTOR 2 ***
-                positie=yT1+yT2;
+            if (MOVAVG_B >= drempelwaardeB1) {
+                yB1=1;
+                if (MOVAVG_B >= drempelwaardeB2) {
+                    yB2=1;
+                    if (MOVAVG_B >= drempelwaardeB3) {
+                        yB3=1;
+                    } else {
+                        yB3=0;
+                    }
+                } else {
+                    yB2=0;
+                }
+            } else {
+                yB1=0;
+            }
 
-                //controle positie op scherm
-                if (positie==0) {
-                    pc.printf("Motor 2 blijft op stand 1\n");
+            pc.printf("Biceps meting is klaar.\n");
+            myled1 = 1;
+            myled2 = 1;
+            myled3 = 0;
+
+            //*** INPUT MOTOR 1 ***
+            snelheidsstand=yB1+yB2+yB3; */
+
+            snelheidsstand=3;
+
+            //controle snelheidsstand op scherm
+            if (snelheidsstand==0) {
+                pc.printf("Motor 1 beweegt niet \n");
+            } else {
+                if (snelheidsstand==1) {
+                    pc.printf("Motor 1 beweegt met snelheid 1 \n");
                 } else {
-                    if (positie==1) {
-                        pc.printf("Motor 2 gaat naar stand 2\n");
+                    if (snelheidsstand==2) {
+                        pc.printf("Motor 1 beweegt met snelheid 2 \n");
                     } else {
-                        if (positie==2) {
-                            pc.printf("Motor 2 gaat naar stand 3\n");
+                        if (snelheidsstand==3) {
+                            pc.printf("Motor 1 beweegt met snelheid 3 \n");
                         }
                     }
                 }
-
-                Ticker looptimer2;
-                looptimer2.attach(motor2aansturing,TSAMP1);
-                wait(8);
-                looptimer2.detach(); 
-                pc.printf("Detach Motor 1\n"); */
-
-//------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
-                wait(2);
-                /* Ticker log_timerB;
-
-                arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
-                arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
-                arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
-
-                myled1 = 1;
-                myled2 = 0;
-                myled3 = 1;
+            }
 
-                log_timerB.attach(Biceps,0.005);
-                wait(2);
-                log_timerB.detach();
-
-                //bepaling van snelheidsstand met biceps
-
-                if (MOVAVG_B >= drempelwaardeB1) {
-                    yB1=1;
-                    if (MOVAVG_B >= drempelwaardeB2) {
-                        yB2=1;
-                        if (MOVAVG_B >= drempelwaardeB3) {
-                            yB3=1;
-                        } else {
-                            yB3=0;
-                        }
-                    } else {
-                        yB2=0;
-                    }
-                } else {
-                    yB1=0;
-                }
+            Ticker looptimer1;
+            //pwm_motor1.write(0.3);
+            motordir1 = 1;
+            stop = 0;
+            looptimer1.attach(motor1aansturing,TSAMP1);
+            wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+            looptimer1.detach();
+            pc.printf("detachMotor1\n");
 
-                pc.printf("Biceps meting is klaar.\n");
-                myled1 = 1;
-                myled2 = 1;
-                myled3 = 0;
-
-                //*** INPUT MOTOR 1 ***
-                snelheidsstand=yB1+yB2+yB3; */
+            Ticker looptimer3;
+            looptimer3.attach(motor1aansturingdeel2,TSAMP1);
+            wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+            looptimer3.detach();
+            pc.printf("detachMotor1\n");
 
-                snelheidsstand=3;
-                
-                //controle snelheidsstand op scherm
-                if (snelheidsstand==0) {
-                    pc.printf("Motor 1 beweegt niet \n");
-                } else {
-                    if (snelheidsstand==1) {
-                        pc.printf("Motor 1 beweegt met snelheid 1 \n");
-                    } else {
-                        if (snelheidsstand==2) {
-                            pc.printf("Motor 1 beweegt met snelheid 2 \n");
-                        } else {
-                            if (snelheidsstand==3) {
-                                pc.printf("Motor 1 beweegt met snelheid 3 \n");
-                            }
-                        }
-                    }
-                }
+            pwm_motor1.write(0);
 
-                Ticker looptimer1;
-                //pwm_motor1.write(0.3);
-                motordir1 = 1;
-                stop = 0;
-                looptimer1.attach(motor1aansturing,TSAMP1);
-                wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
-                looptimer1.detach();
-                pc.printf("detachMotor1\n");
-                
-                Ticker looptimer3;
-                looptimer3.attach(motor1aansturingdeel2,TSAMP1);
-                wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
-                looptimer3.detach();
-                pc.printf("detachMotor1\n");
-                
-                pwm_motor1.write(0);
-                
-                myled1=1;
-                myled2=1;
-                myled3=1;
-        }
+            myled1=1;
+            myled2=1;
+            myled3=1;
         }
     }
+    //}
 }//end int main
 
 
@@ -412,8 +412,7 @@
     float           out_p = 0;
     static float    out_i = 0;
     float           out_d = 0;
-    if(reset==true)
-    {
+    if(reset==true) {
         out_i = 0;
         prev_error = 0;
     }
@@ -629,27 +628,30 @@
     if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
         //motor1.setPosition(0);
-        pid(0,0,true); 
+        pid(0,0,true);
         pc.printf("if1\n");
     }
     if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is
         toestand = SLAAN;
         pc.printf("slaan \n");
         if ( snelheidsstand==3) {
-            setspeed = V3; pc.printf("Snel 3 \n");
+            setspeed = V3;
+            pc.printf("Snel 3 \n");
         }
         if ( snelheidsstand==2) {
-            setspeed = V2; pc.printf("Snel 2\n");
+            setspeed = V2;
+            pc.printf("Snel 2\n");
         }
         if ( snelheidsstand==1) {
-            setspeed = V1; pc.printf("Snel 1 \n");
+            setspeed = V1;
+            pc.printf("Snel 1 \n");
         }
     }
     if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
         toestand = TERUGKEREN;
         //motor1.setPosition(0);
         pwm_motor1.write(0);
-        pid(0,0,true); 
+        pid(0,0,true);
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
         pc.printf("toestand = terugkeren\n\r");
     }
@@ -662,15 +664,18 @@
         pwm_motor1.write(new_pwm);
         motordir1 = 1;
         pc.printf("SLAAN\n");
-        
-    /*if (toestand == TERUGKEREN) {
-        new_pwm = pid(setspeed, motor1.getSpeed(),false); 
-        pwm_motor1.write(new_pwm);
-        motordir1 = 0;
-        pc.printf("motor gaat terugkeren\n\r");
-        pc.printf("new pwm %f\r\n",new_pwm);*/
+        scope.set(0,motor1.getPosition());        //ruwe data
+        scope.set(1,-244);     //filtered
+        scope.send();
+
+        /*if (toestand == TERUGKEREN) {
+            new_pwm = pid(setspeed, motor1.getSpeed(),false);
+            pwm_motor1.write(new_pwm);
+            motordir1 = 0;
+            pc.printf("motor gaat terugkeren\n\r");
+            pc.printf("new pwm %f\r\n",new_pwm);*/
     }
-    
+
 }
 
 void motor1aansturingdeel2()
@@ -678,14 +683,17 @@
     if (motor1.getPosition()>= ANGLEMAX && toestand != SLAAN) {
         toestand = WACHTEN;
         motor1.setPosition(0);
-        pid(0,0,true); 
+        pid(0,0,true);
         //pc.printf("if2\n");
     }
     if (toestand == TERUGKEREN) {
-        new_pwm = pid(setspeed, motor1.getSpeed(),false); 
+        new_pwm = pid(setspeed, motor1.getSpeed(),false);
         pwm_motor1.write(new_pwm);
         motordir1 = 0;
-        
+        //sturen naar HID Scope
+        scope.set(0,motor1.getPosition());        //ruwe data
+        scope.set(2,motor1.getPosition());     //filtered
+        scope.send();
         pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
     }
     if (toestand == WACHTEN) {