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
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 }
Generated on Sat Aug 27 2022 09:58:26 by
1.7.2
