main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 #include "encoder.h"
00004 #include "math.h"
00005 #include "Filter.h"
00006 #include "FilterDesign.h"
00007 #include "Filter2.h"
00008 #include "FilterDesign2.h"
00009 
00010 //D8 doet het niet
00011 
00012 DigitalIn    knop_1(D2); //Motor 1
00013 DigitalIn    knop_2(D3); //Motor 2
00014 
00015 AnalogIn    emg1(A0);
00016 AnalogIn    emg2(A1);
00017 
00018 DigitalOut motor_1(D7); //richting (1 of 0)
00019 PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
00020 DigitalOut motor_2(D4); //richting (1 of 0)
00021 PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
00022 InterruptIn stop(SW3);  //stoppen in case of emergency
00023 InterruptIn beginopnieuw(SW2); //reset het programma zonder op reset te drukken
00024 
00025 Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
00026 
00027 Encoder encoder_1(D13,D12);
00028 Encoder encoder_2(D10,D9);
00029 
00030 Ticker PID_ticker;
00031 Ticker FILTER_ticker;
00032 
00033 Timer tijd;
00034 
00035 //constante waardes
00036 const double x_start = 0.255;
00037 const double dt = 0.001;
00038 const double dt_f = 0.001;
00039 const double Vx = 0.05; //m/s
00040 const double Vy = 0.05; //m/s
00041 const double L2 = 0.35; //m
00042 const double y_base = 0.045;
00043 //filter
00044 volatile double tijd_oud, tijd_nieuw, tijd_verschil;
00045 volatile double u_1, y_1, u_2, y_2; //ongefilerd signaal emg 1, gefilterd signaal emg 1, ongefilterd signaal emg 2, gefilterd signaal emg 2
00046 volatile int mode;   //mode waarin de motors moeten bewegen
00047 //controller
00048 volatile double theta_1, theta_2, reference_1, plaats_1, error_1, pwm_1, reference_2, plaats_2, error_2, pwm_2;
00049 //beginpositie
00050 volatile double x = x_start, y = y_base, positie_motor_1, positie_motor_2; //m
00051 //Voorwaarde voor terugbewegen
00052 volatile double y_oppakken = 0.135, y_stijgen = 0.15, x_zakken = 0.09;
00053 //voorwaarde dat programma aanstaat
00054 volatile bool run_programma = true;
00055 //PID
00056 const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
00057 double m1_v1 = 0, m1_v2 = 0; // Memory variables
00058 const double m1_Ts = 0.001; // Controller sample time
00059 const double m2_Kp = 2, m2_Ki = 5, m2_Kd = 0.1, m2_N = 50;
00060 double m2_v1 = 0, m2_v2 = 0; // Memory variables
00061 const double m2_Ts = 0.001; // Controller sample time
00062 volatile double compensatie = 0;
00063 
00064 
00065 //Controller PID motors
00066 void Controller()
00067 {
00068     theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
00069     theta_2 = acos((x/L2)-cos(theta_1));
00070 
00071     reference_1 = -1.5*theta_1;     //reference
00072     plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
00073     error_1 = reference_1 - plaats_1 ;
00074     pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
00075     if (pwm_1<0){motor_1 = 1;}
00076     else {motor_1 = 0;}
00077     pwm_motor_1 = fabs(pwm_1);
00078 
00079     reference_2 = -1.5*theta_2;     //reference
00080     plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
00081     error_2 = reference_2 - plaats_2 ;
00082     pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 )-compensatie;
00083     if (pwm_2<0){motor_2 = 0;}
00084     else {motor_2 = 1;}
00085     pwm_motor_2 = fabs(pwm_2);
00086 }
00087 //Ticker filterwaardes
00088 void Filter_Samples()
00089 {
00090         u_1 = emg1.read();
00091         y_1 = FilterDesign(u_1);
00092         u_2 = emg2.read();
00093         y_2 = FilterDesign2(u_2);
00094 }
00095 
00096 //Failsave
00097 void Stop() //Zet allebei de snelheden op nul
00098 {
00099     PID_ticker.detach();
00100     pwm_motor_1 = 0;
00101     pwm_motor_2 = 0;
00102     run_programma = false;
00103 }
00104 //Reset (niet nuttig tenzij HIDScope)
00105 void Beginopnieuw()
00106 {
00107     run_programma = true;
00108     x = x_start; y = y_base; m1_v1 = 0; m1_v2 = 0; m2_v1 = 0; m2_v2 = 0;
00109     theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
00110     theta_2 = acos((x/L2)-cos(theta_1));
00111     positie_motor_1 = -1.5*theta_1;
00112     positie_motor_2 = -1.5*theta_2;
00113     encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
00114     encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
00115     PID_ticker.attach(&Controller,dt);
00116     tijd.reset();
00117     tijd.start();
00118 }
00119 
00120 int main()
00121 {
00122     pc.baud(115200);
00123     stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
00124     beginopnieuw.fall(&Beginopnieuw);
00125     pwm_motor_1.period_us(1);
00126     pwm_motor_2.period_us(1);
00127     theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
00128     theta_2 = acos((x/L2)-cos(theta_1));
00129     double positie_motor_1 = -1.5*theta_1;
00130     double positie_motor_2 = -1.5*theta_2;
00131     encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
00132     encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
00133     FILTER_ticker.attach(&Filter_Samples,dt_f);
00134     PID_ticker.attach(&Controller,dt);
00135     wait(1);
00136     tijd.reset();
00137     tijd.start();
00138     while(true)
00139     {
00140     while(run_programma)
00141     {
00142     tijd_nieuw = tijd;
00143     tijd_verschil = tijd_nieuw - tijd_oud;
00144     tijd_oud = tijd_nieuw;
00145    
00146     if      (y_1 >= 0.4 && y_2 <  0.4)                        {mode = 1;} 
00147     else if (y_1 <  0.4 && y_2 >= 0.4)                        {mode = 2;} 
00148     else if (y_1 >= 0.4 && y_2 >= 0.4 && x>(x_start+x_zakken)){mode = 3;}
00149     else if (knop_1 == 0 && knop_2 == 1){mode = 1;} 
00150     else if (knop_1 == 1 && knop_2 == 0){mode = 2;} 
00151     else if (knop_1 == 0 && knop_2 == 0 && x>(x_start+x_zakken)){mode = 3;}
00152     else                             {mode = 4;} //default
00153     
00154     switch (mode)
00155     {
00156         //van de robot af
00157         case 1: x = x + tijd_verschil*Vx;
00158                 if (x>0.6){x = 0.6;}
00159                 y = y_base;
00160                 break;
00161         //naar de robot toe
00162         case 2: x = x - tijd_verschil*Vx;
00163                 if (x<x_start){x = x_start;}
00164                 y = y_base;
00165                 break;
00166         //routine for going back
00167         case 3: //naar beneden
00168                 while(y > (y_base-y_oppakken))    
00169                 {
00170                     tijd_nieuw = tijd;
00171                     tijd_verschil = tijd_nieuw - tijd_oud;        
00172                     y = y - tijd_verschil*Vy*5;
00173                     x = x;
00174                     tijd_oud = tijd_nieuw;
00175                 }
00176                 wait(0.05);
00177                 tijd_nieuw = tijd;
00178                 tijd_oud = tijd_nieuw;
00179                 //naar boven
00180                 while(y < (y_base+y_stijgen))
00181                 {
00182                     tijd_nieuw = tijd;
00183                     tijd_verschil = tijd_nieuw - tijd_oud;        
00184                     y = y + tijd_verschil*Vy*5;
00185                     x = x;
00186                     tijd_oud = tijd_nieuw;
00187                 }
00188                 //naar de robot toe toe zakpositie
00189                 while(x > (x_start+x_zakken))
00190                 {
00191                     tijd_nieuw = tijd;
00192                     tijd_verschil = tijd_nieuw - tijd_oud;        
00193                     x = x-tijd_verschil*Vx*2;
00194                     tijd_oud = tijd_nieuw;
00195                 }
00196                 //naar uitdrukhoogte
00197                 while(y>y_base)
00198                 {
00199                     tijd_nieuw = tijd;
00200                     tijd_verschil = tijd_nieuw - tijd_oud;    
00201                     y = y - tijd_verschil*Vy*2;
00202                     x = x;
00203                     tijd_oud = tijd_nieuw;
00204                 }
00205                 //uitdrukken
00206                 while(x>x_start-0.03)
00207                 {
00208                     tijd_nieuw = tijd;
00209                     tijd_verschil = tijd_nieuw - tijd_oud;   
00210                     y = y_base;
00211                     x = x - tijd_verschil*Vx*2;
00212                     if (x<x_start)
00213                     {
00214                     compensatie = 0.3;
00215                     }
00216                     tijd_oud = tijd_nieuw;
00217                 }
00218                 compensatie = 0;
00219                 wait(0.5);
00220                     x = x_start;
00221                     y = y_base;
00222                 break;
00223     
00224         break;
00225         //doe niks
00226         default: x = x;
00227                  y = y;
00228                  break;
00229     }    
00230     }
00231     }
00232 }