Hier Roodmans
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 #include "encoder.h" 00004 #include "QEI.h" 00005 00006 DigitalIn button1(D8); 00007 DigitalIn button2(D9); 00008 //AnalogIn potMeterIn(A1); 00009 float potMeterIn; // = 0.01; snelheid in, zonder potmeter gebruik 00010 00011 DigitalOut motor1DirectionPin(D4); 00012 PwmOut motor1MagnitudePin(D5); 00013 00014 QEI Encoder(D12,D13,NC,32); 00015 00016 00017 int counts = Encoder.getPulses(); 00018 int delta; 00019 DigitalOut led(LED_RED); 00020 DigitalOut ledd(LED_GREEN); 00021 DigitalOut leddd(LED_BLUE); 00022 InterruptIn button(SW2); 00023 Timer t; 00024 Serial pc(USBTX, USBRX); 00025 00026 int Xin=-5; 00027 /*float huidigetijdX; 00028 void ledtX(){ 00029 t.reset(); 00030 Xin++; 00031 pc.printf("Xin is %i\n",Xin); 00032 ledd=0; 00033 led=1; 00034 wait(0.5); 00035 ledd=1; 00036 led=0; 00037 } 00038 00039 int tellerX(){ 00040 ledd=1; 00041 leddd=1; 00042 while(true){ 00043 button.fall(ledtX); 00044 /*if (EMG>threshold){ 00045 ledtX(); // dit is wat je uiteindelijk wil dat er staat 00046 }*//* 00047 t.start(); 00048 huidigetijdX=t.read(); 00049 if (huidigetijdX>2){ 00050 led=1; // ga door naar het volgende programma 00051 return 0; 00052 } 00053 } 00054 } 00055 */int Yin=-3;/* 00056 float huidigetijdY; 00057 void ledtY(){ 00058 t.reset(); 00059 Yin++; 00060 pc.printf("Yin is %i\n",Yin); 00061 ledd=0; 00062 leddd=1; 00063 wait(0.5); 00064 ledd=1; 00065 leddd=0; 00066 } 00067 00068 int tellerY(){ 00069 t.reset(); 00070 ledd=1; 00071 leddd=0; 00072 while(true){ 00073 button.fall(ledtY); 00074 /*if (EMG>threshold){ 00075 Piek(); // dit is wat je uiteindelijk wil dat er staat 00076 }*//* 00077 t.start(); 00078 huidigetijdY=t.read(); 00079 if (huidigetijdY>2){ 00080 leddd=1; 00081 button.fall(0); 00082 return 0; // ga door naar het volgende programma 00083 } 00084 } 00085 }*/ 00086 int B; 00087 float Pox = 0; 00088 float Poy = 0; 00089 float Pbx = 0; 00090 float Pby = 887; 00091 float Prx = 768; 00092 float Pry = 443; 00093 float Pex=121; 00094 float Pey=308; 00095 float diamtrklosje=20; 00096 float pi=3.14159265359; 00097 float omtrekklosje=diamtrklosje*pi; 00098 float Lou; 00099 float Lbu; 00100 float Lru; 00101 float dLod; 00102 float dLbd; 00103 float dLrd; 00104 float roto; 00105 float rotb; 00106 float rotr; 00107 float rotzo; 00108 float rotzb; 00109 float rotzr; 00110 float counto; 00111 float countb; 00112 float countr; 00113 float countzo; 00114 float countzb; 00115 float countzr; 00116 float Psx; 00117 float Psy; 00118 float Vex; 00119 float Vey; 00120 float Kz=0.7; // nadersnelheid instellen 00121 float modVe; 00122 float Vmax=20; 00123 float Pstx; 00124 float Psty; 00125 float T=0.02;//seconds 00126 00127 00128 //Deel om motor(en) aan te sturen-------------------------------------------- 00129 00130 void calcdelta(void) { 00131 delta = (counto - Encoder.getPulses()); 00132 } 00133 00134 //Encoder motor1(D12,D13); //Gear ratio = encoder ratio = 131.25:1 & 00135 // factor 64: To compute the counts per revolution of the gearbox output, multiply the gear ratio by 64. 00136 00137 float referenceVelocity; 00138 float motorValue; 00139 00140 Ticker controlmotor1; // één ticker van maken? 00141 Ticker calculatedelta; 00142 Ticker feedbacktick; 00143 Ticker printdata; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes 00144 00145 float GetReferenceVelocity() 00146 { 00147 // Returns reference velocity in rad/s. 00148 // Positive value means clockwise rotation. 00149 float maxVelocity=Vex*25+Vey*25; // max 8.4 in rad/s of course! 00150 referenceVelocity = (-1)*potMeterIn * maxVelocity; 00151 00152 if (Encoder.getPulses() < (counto+1)) 00153 { potMeterIn = 0.1; 00154 } 00155 else if (Encoder.getPulses() > (counto-1)) 00156 { potMeterIn = -0.1; 00157 } 00158 else 00159 { potMeterIn = 0; 00160 } 00161 00162 return referenceVelocity; 00163 } 00164 00165 00166 void SetMotor1(float motorValue) 00167 { 00168 // Given -1<=motorValue<=1, this sets the PWM and direction 00169 // bits for motor 1. Positive value makes motor rotating 00170 // clockwise. motorValues outside range are truncated to 00171 // within range 00172 if (motorValue >=0) motor1DirectionPin=1; 00173 else motor1DirectionPin=0; 00174 if (fabs(motorValue)>1) motor1MagnitudePin = 1; 00175 else motor1MagnitudePin = fabs(motorValue); 00176 00177 } 00178 00179 float FeedForwardControl(float referenceVelocity) 00180 { 00181 // very simple linear feed-forward control 00182 const float MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 00183 float motorValue = referenceVelocity / MotorGain; 00184 return motorValue; 00185 } 00186 00187 void MeasureAndControl() 00188 { 00189 // This function measures the potmeter position, extracts a 00190 // reference velocity from it, and controls the motor with 00191 // a simple FeedForward controller. Call this from a Ticker. 00192 float referenceVelocity = GetReferenceVelocity(); 00193 float motorValue = FeedForwardControl(referenceVelocity); 00194 SetMotor1(motorValue); 00195 } 00196 00197 void readdata() 00198 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); 00199 pc.printf("Pulses = %i \r\n",Encoder.getPulses()); 00200 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); 00201 pc.printf("Delta = %i \r\n",delta); 00202 } 00203 00204 // einde deel motor------------------------------------------------------------------------------------ 00205 00206 Ticker loop; 00207 float touwlengtes(){ 00208 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2)); // rekent touwlengtes uit die nodig zijn voor de nieuwe positie aan de hand van de ingegeven coördinaten en de posities van de palen 00209 Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2)); 00210 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2)); 00211 return 0; 00212 } 00213 float turns(){ 00214 00215 roto=Lou/omtrekklosje; 00216 rotb=Lbu/omtrekklosje; 00217 rotr=Lru/omtrekklosje; 00218 counto=roto*4200; 00219 //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts 00220 countb=rotb*4200; 00221 //countb = (int)(countb + 0.5); 00222 countr=rotr*4200; 00223 //countr = (int)(countr + 0.5); 00224 return 0; 00225 } 00226 00227 00228 float Pst(){ 00229 Pstx=Pex+Vex*T; 00230 Psty=Pey+Vey*T; 00231 touwlengtes(); 00232 Pex=Pstx; 00233 Pey=Psty; 00234 //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty); 00235 //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru); 00236 turns(); 00237 //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); 00238 pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr); 00239 /*float R; 00240 R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constand is en de end efector dus een rechte lijn maakt 00241 pc.printf("\n\r R=%f",R);*/ 00242 return 0; 00243 } 00244 00245 float Ps(){ 00246 Psx=(Xin)*30+121; 00247 Psy=(Yin)*30+308; 00248 pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); 00249 return 0; 00250 } 00251 void Ve(){ 00252 Vex=Kz*(Psx-Pex); 00253 Vey=Kz*(Psy-Pey); 00254 modVe=sqrt(pow(Vex,2)+pow(Vey,2)); 00255 if(modVe>Vmax){ 00256 Vex=(Vex/modVe)*Vmax; 00257 Vey=(Vey/modVe)*Vmax; 00258 } 00259 Pst(); 00260 pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); 00261 if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ 00262 B=5; 00263 loop.detach(); 00264 } 00265 } 00266 00267 int calculator(){ 00268 Ps(); 00269 loop.attach(&Ve,0.02); 00270 return 0; 00271 } 00272 00273 void zakker(){ 00274 while(1){ 00275 wait(1); 00276 if(B==5){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is 00277 dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken 00278 dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu; 00279 dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru; 00280 rotzo=dLod/omtrekklosje; 00281 rotzb=dLbd/omtrekklosje; 00282 rotzr=dLrd/omtrekklosje; 00283 countzo=rotzo*4200; 00284 countzb=rotzb*4200; 00285 countzr=rotzr*4200; 00286 00287 pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat 00288 } 00289 } 00290 } 00291 00292 int main() 00293 { 00294 pc.baud(115200); 00295 //tellerX(); 00296 //tellerY(); 00297 calculator(); 00298 controlmotor1.attach(&MeasureAndControl, 0.01); 00299 calculatedelta.attach(&calcdelta, 0.01); 00300 printdata.attach(&readdata, 0.5); 00301 //zakker(); 00302 00303 return 0; 00304 }
Generated on Fri Jul 22 2022 05:14:09 by
1.7.2