Diff: main.cpp

Revision:

8:b219ca30967f

Parent:

7:22126f285d69

Child:

9:888ed3c72795

--- a/main.cpp	Thu Oct 22 13:55:28 2015 +0000
+++ b/main.cpp	Thu Oct 22 15:07:20 2015 +0000
@@ -3,40 +3,36 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 
+
+// pins
 DigitalOut  motor1_direction(D4);
 PwmOut      motor1_speed(D5);
-PwmOut      led(D9);
 DigitalIn   button_1(PTC6); //counterclockwise
 DigitalIn   button_2(PTA4); //clockwise
-AnalogIn    PotMeter1(A5);
+AnalogIn    PotMeter1(A4);
 AnalogIn    EMG(A0);
-//AnalogIn    EMG_bicepsright (A1);
-//AnalogIn    EMG_legleft (A2);
-//AnalogIn    EMG_legright (A3);
 Ticker      controller;
 Ticker      ticker_regelaar;
 Ticker      EMG_Filter;
 Ticker      Scope;
-//Ticker      Encoder_Scope;
-//Timer       Timer_Calibration;
 Encoder     motor1(D12,D13);
 HIDScope    scope(3);
+MODSERIAL   pc(USBTX, USBRX);
 
-MODSERIAL   pc(USBTX, USBRX);
+
+
+// Regelaar homeposition
+#define SAMPLETIME_REGELAAR 0.005 
 volatile bool regelaar_ticker_flag;
-
-void setregelaar_ticker_flag()
-{
+void setregelaar_ticker_flag(){
     regelaar_ticker_flag = true;
 }
 
-#define SAMPLETIME_REGELAAR 0.005
-
 //define states
-enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
+enum state {HOME, MOVE_MOTOR_1, BACKTOHOMEPOSITION, STOP};
 uint8_t state = HOME;
 
-// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
+// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
 const double g = 360; // Aantal graden 1 rotatie
 const double c = 4200; // Aantal counts 1 rotatie
 const double q = c/(g);
@@ -48,37 +44,34 @@
 double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
 
 // Reusable P controller
-double Pc (double error, const double Kp)
-{
+double Pc (double error, const double Kp){
     return motor1_Kp * error;
 }
 
 // Measure the error and apply output to the plant
-void motor1_controlP()
-{
+void motor1_controlP(){
     double referenceP1 = PotMeter1.read();
     double positionP1 = q*motor1.getPosition();
     double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
 }
 
 // Reusable PI controller
-double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
-{
+double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int){
     err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
     return motor1_Kp*error + motor1_Ki*err_int;
 } // De totale fout die wordt hersteld met behulp van PI control.
 
-//bool Cali = false;
-//double TimeCali = 5;
 
 // Filter1 = High pass filter tot 20 Hz
 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
 const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
+
 // Filter2 = Low pass filter na 60 Hz
 double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
 const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
 const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
+
 // Filter3 = Notch filter at 50 Hz
 double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
@@ -94,9 +87,7 @@
 double final_filter1;
 
 // Standaard formule voor het biquad filter
-double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
-
-{
+double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){
     double v = u - a1*v1 - a2*v2;
     double y = b0*v + b1*v1 + b2*v2;
     v2=v1;
@@ -104,8 +95,8 @@
     return y;
 }
 
-void Filters()
-{
+// script voor filters
+void Filters(){
     u1 = EMG.read();
     //Highpass
     y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
@@ -132,8 +123,7 @@
 } 
  
 
-void motor1_controlPI()
-{
+void motor1_controlPI(){
     double referencePI1 = PotMeter1.read();
     double positionPI1 = q*motor1.getPosition();
     double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
@@ -141,23 +131,28 @@
 
 const int pressed = 0;
 
+// constantes voor berekening homepositie
 double H;
 double P;
 double D;
 
 
-void gethome(){
-    H = motor1.getPosition();
-}
-
-void move_motor1_ccw (){
-    motor1_direction = 0;
-    motor1_speed = 0.4;
-}
-
-void move_motor1_cw (){
-    motor1_direction = 1;
-    motor1_speed = 0.4;
+void move_motor1()
+{
+        if (final_filter1 > 0.03){
+            pc.printf("Moving clockwise \n\r");
+            motor1_direction = 1; //clockwise
+            motor1_speed = 0.4;
+            }
+        else if (button_2 == pressed){
+            pc.printf("Moving counterclockwise \n\r");
+            motor1_direction = 0; //counterclockwise
+            motor1_speed = 0.4;
+            }
+        else {
+            pc.printf("Not moving \n\r"); 
+            motor1_speed = 0;
+            }
 }
 
 void movetohome(){
@@ -176,90 +171,38 @@
     }
 }
 
-void move_motor1()
-{
-        if (final_filter1 > 0.03){
-            pc.printf("Moving clockwise \n\r");
-            move_motor1_cw ();
-            }
-        else if (button_2 == pressed){
-            pc.printf("Moving counterclockwise \n\r");
-            move_motor1_ccw ();
-            }
-        else {
-            pc.printf("Not moving \n\r"); 
-            motor1_speed = 0;
-            }
-}
-
-//void read_encoder1 ()    // aflezen van encoder via hidscope??
-//{
-//    scope.set(0,motor1.getPosition());
-    //led.write(motor1.getPosition()/100.0);
-//    scope.send();
-//    wait(0.2f);
-//}
-
 void HIDScope (){
-      scope.set (0, y8);
-   // scope.set (1, y2);
-   // scope.set (2, y4);
-   // scope.set (3, y7);
-   scope.set (1, final_filter1);
-   //scope.set (2, final_filter1); 
-   scope.set(2, motor1.getPosition());
+    scope.set (0, final_filter1);
+    scope.set(1, motor1.getPosition());
     scope.send ();
     }
     
-
-
 int main()
 {    
     while (true) {
-        pc.baud(9600);          //pc baud rate van de computer
-        EMG_Filter.attach_us(Filters, 1e3);
-        Scope.attach_us(HIDScope, 1e3);
+        pc.baud(9600);                          //pc baud rate van de computer
+        EMG_Filter.attach_us(Filters, 1e3);     //filters uitvoeren
+        Scope.attach_us(HIDScope, 1e3);         //EMG en encoder signaal naar de hidscope sturen     
         
-    switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
+    switch (state) {                            //zorgt voor het in goede volgorde uitvoeren van de cases
         
         case HOME:      //positie op 0 zetten voor arm 1
          {
             pc.printf("home\n\r");
-            //read_encoder1();
-            gethome();
+            H = motor1.getPosition();
             pc.printf("Home-position is %f\n\r", H);
-            state = MOVE_MOTOR;
-            wait(5);
+            state = MOVE_MOTOR_1;
+            wait(2);
             break;
         }
-        
-        //case CALIBRATIE:
-        //{        
-            //pc.printf("Aanspannen in 10 \n\r");
-            //wait(10);
-            //EMG_Control.attach_us(MyController,1e3);
-            //Timer_Calibration.start();
-            //if (Timer_Calibration < TimeCali) {
-            //    Cali = true;
-            //    pc.printf("Aanspannen \n\r");
-            //} 
-            //else {
-            //    Cali = false;
-            //}  
-            //pc.printf("Stoppen \n\r"); 
-            //Timer_Calibration.stop();
-            //Timer_Calibration.reset();
-            //state = MOVE_MOTOR;
-          //  break;
-        //}
-        
-        case MOVE_MOTOR:            //motor kan cw en ccw bewegen a.d.h.v. buttons
+      
+        case MOVE_MOTOR_1:            //motor kan cw en ccw bewegen a.d.h.v. buttons
         {
             pc.printf("move_motor\n\r");
-            while (state == MOVE_MOTOR){
+            while (state == MOVE_MOTOR_1){
             move_motor1();
-            //print_position();
             if (button_1 == pressed && button_2 == pressed){
+            motor1_speed = 0;
             state = BACKTOHOMEPOSITION;
             }
             }
@@ -273,8 +216,6 @@
             wait (1);
             
             ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
-            //EMG_Control.attach_us(Filters,1e3);
-
             while(state == BACKTOHOMEPOSITION){
                 movetohome();
                 while(regelaar_ticker_flag != true);