Hier Roodmans

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }