Motor 1 is perfectly working, but however if I add motor 2 the whole thing is getting weird. The return to home is perfectly working.

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of Motor4_SETPOINTPD by Fernon Eijkhoudt

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "math.h"
00004 #include "HIDScope.h"
00005 
00006 
00007 
00008 // Motor 1 & 2
00009 DigitalOut      Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
00010 PwmOut          PowerMotor(D5); //van 0 tot 1
00011 QEI             Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder
00012 DigitalOut      Direction2(D6);
00013 PwmOut          PowerMotor2(D7);
00014 QEI             Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
00015 PwmOut          PowerServo(D3);
00016 
00017 // Buttons & EMG (PotMeter)
00018 DigitalIn       Button(PTC6);
00019 DigitalIn       Button2(PTA4);
00020 AnalogIn        PotMeter(A1);
00021 AnalogIn        PotMeter2(A2);
00022 //AnalogIn        EMG(A0);
00023 //AnalogIn        Emg(A1);
00024 
00025 // Tickers & timers
00026 Ticker          MotorWrite;
00027 Ticker          Sender;
00028 Ticker          sampleEMG;
00029 Timer           timer;
00030 
00031 // Debugging
00032 Serial          pc(USBTX, USBRX);
00033 HIDScope        scope(3);
00034 
00035 
00036 
00037 // Waardes
00038 const double    twopi = 6.2831853071795;
00039 const double    Fs=100;
00040 int             Fired = 0;
00041 
00042 // EMG
00043 double          emg_value;
00044 double          emg_value2;
00045 const double    T1 = 0.33333; // Treshold 1
00046 const double    T2 = 0.66666; // Treshold 2
00047 
00048 // Motor 1 (Translatie)
00049 double          n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
00050 int             Pulses;
00051 double          Rotatie = 0; //aantal graden dat de motor is gedraaid
00052 double          Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
00053 double          Error = 0;
00054 double          Errord = 0;
00055 double          Errori = 0;
00056 double          Errorp = 0;
00057 const double    Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
00058 const double    Kd = 10;
00059 const double    Ki = 0.2;
00060 double          v = 0; //snelheid van de motor (0-0.1)?
00061 double          upperlimit; //max aantal rotaties omhoog
00062 const double    downlimit = 0.4;
00063 const double    margin = 0.4;
00064 bool            OutRange = false;
00065 
00066 // Motor 2 (Rotatie)
00067 double          n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
00068 int             Pulses2;
00069 double          Rotatie2 = 0;
00070 double          Goal2;
00071 double          Error2 = 0;
00072 double          Errord2 = 0;
00073 double          Errori2 = 0;
00074 double          Errorp2 = 0;
00075 const double    Kp2 = 0.2;
00076 const double    Kd2 = 10;
00077 const double    Ki2 = 0.2;
00078 double          v2 = 0;
00079 double          turnlimit; // max aantal rotaties voor roteren hele robot
00080 // Margin 2 is in ons geval 0
00081 bool            OutRange2 = false;
00082 
00083 // Activatie tussen het schietgedeelte en terugkeergedeelte
00084 bool            Excecute = false;
00085 bool            Home = false;
00086 
00087 
00088 
00089 // Voids voor berekeningen in het hoofdprogramma
00090 
00091 void MotorSet()
00092 {
00093     // Eerst motor 1 (translatie)
00094     if (OutRange) {
00095         Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
00096         Errord = (Error-Errorp)/Fs;
00097         Errorp = Error;
00098         if (fabs(Error) <= 0.5) {
00099             Errori = Errori + Error*1/Fs;
00100         } else {
00101             Errori = 0;
00102         }
00103         if (Error>=0) {
00104             Direction=1;
00105         } else {
00106             Direction=0;
00107         }
00108         v=Kp*Error + Kd*Errord + Ki*Errori;
00109     }
00110     PowerMotor.write(fabs(v));
00111 
00112     // Dan motor 2 (rotatie)
00113     if (OutRange2) {
00114         Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
00115         Errord2 = (Error2-Errorp2)/Fs;
00116         Errorp2 = Error2;
00117         if (fabs(Error2) <= 0.5) {
00118             Errori2 = Errori2 + Error2*1/Fs;
00119         } else {
00120             Errori2 = 0;
00121         }
00122         if (Error2>=0) {
00123             Direction=1;
00124         } else {
00125             Direction=0;
00126         }
00127         v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
00128     }
00129     PowerMotor2.write(fabs(v2));
00130 }
00131 void Send()
00132 {
00133     Pulses = Encoder.getPulses();
00134     Rotatie = (Pulses*twopi)/4200; 
00135     Pulses2 = Encoder2.getPulses();
00136     Rotatie2 = (Pulses2*twopi)/4200;
00137     scope.set(0,Goal);
00138     scope.set(1,Rotatie);
00139     scope.set(2,emg_value);
00140     scope.send();
00141 }
00142 void EMGsample()
00143 {
00144     // Lees het EMG signaal uit
00145     //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
00146     emg_value = PotMeter.read();
00147     emg_value2 = PotMeter2.read();
00148 }
00149 
00150 void Fire ()
00151 {
00152     PowerServo.write(0.27);
00153     wait (1);
00154     PowerServo.write(0.04);
00155     wait (1);
00156     Fired=Fired+1;
00157     if (Fired >= 3) {
00158         wait (1);
00159         Excecute = false;
00160         Home = true;
00161         Fired = 0;
00162     }
00163 }
00164 
00165 
00166 // Calibratie moet nog worden uitgevoerd!!!!
00167 
00168 
00169 int main()
00170 {
00171     upperlimit = n1*twopi;
00172     turnlimit = n2*twopi;
00173     pc.baud(115200);
00174     PowerMotor.write(0);
00175     Sender.attach(Send,0.5/Fs);
00176     MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
00177     sampleEMG.attach(EMGsample,0.5/Fs);
00178     PowerServo.period_ms(20);
00179     while (true) {
00180         Encoder.reset();
00181         Encoder2.reset();
00182         if (Button == 0) {
00183             Excecute =! Excecute;
00184         }
00185         while (Excecute ) {
00186             // Eerst wordt motor 1 aangestuurd
00187             pc.printf("Rotatie = %f \n", Rotatie); 
00188             pc.printf("Rotatie2 = %f \n", Rotatie2);
00189             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
00190                 Goal = upperlimit - margin;
00191                 OutRange = true;
00192             }
00193             if (Rotatie <= downlimit) { //Als hij buiten bereik is
00194                 Goal = 0 + margin;
00195                 OutRange = true;
00196             }
00197             if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
00198                 OutRange = false;
00199             }
00200             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
00201                 if (emg_value >= T2 ) {
00202                     Direction = 1;
00203                     v = 1;
00204                 }
00205                 if (emg_value >= T1 && emg_value <= T2) {
00206                     Direction = 0;
00207                     v = 1;
00208                 }
00209                 if (emg_value <= T1) {
00210                     Direction = 0;
00211                     v = 0;
00212                 }
00213             }
00214 
00215             //// Vanaf hier wordt motor 2 aangestuurd
00216 //            if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
00217 //                Goal2 = turnlimit;
00218 //                OutRange2 = true;
00219 //            }
00220 //            if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
00221 //                Goal2 = -turnlimit;
00222 //                OutRange2 = true;
00223 //            }
00224 //            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
00225 //                OutRange2 = false;
00226 //            }
00227 //            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
00228 //
00229 //                if (emg_value2 >= T2 ) {
00230 //                    Direction = 1;
00231 //                    v = 1;
00232 //                }
00233 //                if (emg_value2 >= T1 && emg_value2 <= T2) {
00234 //                    Direction = 0;
00235 //                    v = 1;
00236 //                }
00237 //                if (emg_value2 <= T1) {
00238 //                    Direction = 0;
00239 //                    v = 0;
00240 //                }
00241 //            }
00242             if (Button2 == 0) { //Afvuren van de RBG
00243                 Fire();
00244             }
00245         }
00246 
00247         while (Home) { //Terugkeren naar vaste positie
00248             OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
00249             Goal = 0;
00250             Goal2 = 0;
00251             if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
00252                 timer.start();
00253             } else {
00254                 timer.stop();
00255                 timer.reset();
00256             }
00257             if (timer.read() >= 3) {
00258                 Home = false;
00259                 Errori = 0;
00260                 Errord = 0;
00261                 Errorp = 0;
00262                 Errori2 = 0;
00263                 Errord2 = 0;
00264                 Errorp2 = 0;
00265             }
00266         }
00267     }
00268 }