Jitse Giesen
/
Koppeling
Hier Roodmans
main.cpp
- Committer:
- Jitse_Giesen
- Date:
- 2017-10-24
- Revision:
- 5:aa191ca932ab
- Parent:
- 4:050dd85ffcf5
File content as of revision 5:aa191ca932ab:
#include "mbed.h" #include "math.h" #include "encoder.h" #include "QEI.h" DigitalIn button1(D8); DigitalIn button2(D9); //AnalogIn potMeterIn(A1); float potMeterIn; // = 0.01; snelheid in, zonder potmeter gebruik DigitalOut motor1DirectionPin(D4); PwmOut motor1MagnitudePin(D5); QEI Encoder(D12,D13,NC,32); int counts = Encoder.getPulses(); int delta; DigitalOut led(LED_RED); DigitalOut ledd(LED_GREEN); DigitalOut leddd(LED_BLUE); InterruptIn button(SW2); Timer t; Serial pc(USBTX, USBRX); int Xin=-5; /*float huidigetijdX; void ledtX(){ t.reset(); Xin++; pc.printf("Xin is %i\n",Xin); ledd=0; led=1; wait(0.5); ledd=1; led=0; } int tellerX(){ ledd=1; leddd=1; while(true){ button.fall(ledtX); /*if (EMG>threshold){ ledtX(); // dit is wat je uiteindelijk wil dat er staat }*//* t.start(); huidigetijdX=t.read(); if (huidigetijdX>2){ led=1; // ga door naar het volgende programma return 0; } } } */int Yin=-3;/* float huidigetijdY; void ledtY(){ t.reset(); Yin++; pc.printf("Yin is %i\n",Yin); ledd=0; leddd=1; wait(0.5); ledd=1; leddd=0; } int tellerY(){ t.reset(); ledd=1; leddd=0; while(true){ button.fall(ledtY); /*if (EMG>threshold){ Piek(); // dit is wat je uiteindelijk wil dat er staat }*//* t.start(); huidigetijdY=t.read(); if (huidigetijdY>2){ leddd=1; button.fall(0); return 0; // ga door naar het volgende programma } } }*/ int B; float Pox = 0; float Poy = 0; float Pbx = 0; float Pby = 887; float Prx = 768; float Pry = 443; float Pex=121; float Pey=308; float diamtrklosje=20; float pi=3.14159265359; float omtrekklosje=diamtrklosje*pi; float Lou; float Lbu; float Lru; float dLod; float dLbd; float dLrd; float roto; float rotb; float rotr; float rotzo; float rotzb; float rotzr; float counto; float countb; float countr; float countzo; float countzb; float countzr; float Psx; float Psy; float Vex; float Vey; float Kz=0.7; // nadersnelheid instellen float modVe; float Vmax=20; float Pstx; float Psty; float T=0.02;//seconds //Deel om motor(en) aan te sturen-------------------------------------------- void calcdelta(void) { delta = (counto - Encoder.getPulses()); } //Encoder motor1(D12,D13); //Gear ratio = encoder ratio = 131.25:1 & // factor 64: To compute the counts per revolution of the gearbox output, multiply the gear ratio by 64. float referenceVelocity; float motorValue; Ticker controlmotor1; // één ticker van maken? Ticker calculatedelta; Ticker feedbacktick; Ticker printdata; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes float GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. float maxVelocity=Vex*25+Vey*25; // max 8.4 in rad/s of course! referenceVelocity = (-1)*potMeterIn * maxVelocity; if (Encoder.getPulses() < (counto+1)) { potMeterIn = 0.1; } else if (Encoder.getPulses() > (counto-1)) { potMeterIn = -0.1; } else { potMeterIn = 0; } return referenceVelocity; } void SetMotor1(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) motor1DirectionPin=1; else motor1DirectionPin=0; if (fabs(motorValue)>1) motor1MagnitudePin = 1; else motor1MagnitudePin = fabs(motorValue); } float FeedForwardControl(float referenceVelocity) { // very simple linear feed-forward control const float MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 float motorValue = referenceVelocity / MotorGain; return motorValue; } void MeasureAndControl() { // This function measures the potmeter position, extracts a // reference velocity from it, and controls the motor with // a simple FeedForward controller. Call this from a Ticker. float referenceVelocity = GetReferenceVelocity(); float motorValue = FeedForwardControl(referenceVelocity); SetMotor1(motorValue); } void readdata() { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); pc.printf("Pulses = %i \r\n",Encoder.getPulses()); //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); pc.printf("Delta = %i \r\n",delta); } // einde deel motor------------------------------------------------------------------------------------ Ticker loop; float touwlengtes(){ 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 Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2)); Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2)); return 0; } float turns(){ roto=Lou/omtrekklosje; rotb=Lbu/omtrekklosje; rotr=Lru/omtrekklosje; counto=roto*4200; //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts countb=rotb*4200; //countb = (int)(countb + 0.5); countr=rotr*4200; //countr = (int)(countr + 0.5); return 0; } float Pst(){ Pstx=Pex+Vex*T; Psty=Pey+Vey*T; touwlengtes(); Pex=Pstx; Pey=Psty; //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty); //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru); turns(); //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr); /*float R; 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 pc.printf("\n\r R=%f",R);*/ return 0; } float Ps(){ Psx=(Xin)*30+121; Psy=(Yin)*30+308; pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); return 0; } void Ve(){ Vex=Kz*(Psx-Pex); Vey=Kz*(Psy-Pey); modVe=sqrt(pow(Vex,2)+pow(Vey,2)); if(modVe>Vmax){ Vex=(Vex/modVe)*Vmax; Vey=(Vey/modVe)*Vmax; } Pst(); pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ B=5; loop.detach(); } } int calculator(){ Ps(); loop.attach(&Ve,0.02); return 0; } void zakker(){ while(1){ wait(1); if(B==5){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu; dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru; rotzo=dLod/omtrekklosje; rotzb=dLbd/omtrekklosje; rotzr=dLrd/omtrekklosje; countzo=rotzo*4200; countzb=rotzb*4200; countzr=rotzr*4200; pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat } } } int main() { pc.baud(115200); //tellerX(); //tellerY(); calculator(); controlmotor1.attach(&MeasureAndControl, 0.01); calculatedelta.attach(&calcdelta, 0.01); printdata.attach(&readdata, 0.5); //zakker(); return 0; }