Script voor het aansturen van de motor met behulp van twee potmeters. Potmeter 1 wordt gebruikt om de draairichting te bepalen en potmeter 2 wordt gebruikt om de snelheid te bepalen. Het script werkt en heeft een snellere reactie op de potmeters dan bij het vorige script.
Dependencies: HIDScope mbed Encoder MODSERIAL
Fork of motor_met_potmeters_en_aanuit by
main.cpp@8:432f6d4669e1, 2017-10-09 (annotated)
- Committer:
- paulineoonk
- Date:
- Mon Oct 09 14:33:20 2017 +0000
- Revision:
- 8:432f6d4669e1
- Parent:
- 7:df5ad7a84f86
- Child:
- 9:9a356a676a7c
Leven is zo zwaaaaarrr, moeilijk,; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Annelotte | 0:85965fd37d39 | 1 | #include "mbed.h" |
Annelotte | 4:fbf4307e8aea | 2 | #include "HIDScope.h" |
paulineoonk | 8:432f6d4669e1 | 3 | #include "encoder.h" |
paulineoonk | 8:432f6d4669e1 | 4 | #include "MODSERIAL.h" |
Annelotte | 4:fbf4307e8aea | 5 | |
paulineoonk | 8:432f6d4669e1 | 6 | |
Annelotte | 4:fbf4307e8aea | 7 | Ticker AInTicker; //We maken een ticker aan genaamd AIn (wordt gebruikt voor HIDScope) |
Annelotte | 4:fbf4307e8aea | 8 | |
Annelotte | 0:85965fd37d39 | 9 | Ticker Treecko; //We maken een geweldige ticker aan zodat de potmeter en alles telkens opnieuw worden uitgelezen |
paulineoonk | 7:df5ad7a84f86 | 10 | AnalogIn potMeter2(A1); //Analoge input van potmeter 2 (zal gebruikt worden voor de snelheids bepaling van de motor) |
paulineoonk | 7:df5ad7a84f86 | 11 | AnalogIn potMeter1(A3); //Analoge input van potmeter 1 (zal gebruikt worden voor de bepaling van de draairichting van de motor) |
paulineoonk | 7:df5ad7a84f86 | 12 | PwmOut M1E(D6); //Biorobotics Motor 1 PWM controle van de snelheid |
paulineoonk | 7:df5ad7a84f86 | 13 | DigitalOut M1D(D7); //Biorobotics Motor 1 draairichting controle |
paulineoonk | 8:432f6d4669e1 | 14 | //gitalIn enc1ledB(D12); //Led B van de encoder van motor 1 |
paulineoonk | 8:432f6d4669e1 | 15 | //gitalIn enc1ledA(D13); //Led A van de encoder van motor 1 |
Annelotte | 0:85965fd37d39 | 16 | |
paulineoonk | 8:432f6d4669e1 | 17 | Encoder motor1(D13,D12,true); |
paulineoonk | 8:432f6d4669e1 | 18 | MODSERIAL pc(USBTX,USBRX); |
paulineoonk | 8:432f6d4669e1 | 19 | |
Annelotte | 4:fbf4307e8aea | 20 | |
Annelotte | 4:fbf4307e8aea | 21 | |
Annelotte | 0:85965fd37d39 | 22 | float PwmPeriod = 1.0/5000.0; //PWM periode instellen (5000 Hz, want 5000 periodes in 1 seconde) |
Annelotte | 0:85965fd37d39 | 23 | |
Annelotte | 3:e1147e50344b | 24 | void SetMotor1(float motorValue) |
Annelotte | 0:85965fd37d39 | 25 | { |
Annelotte | 0:85965fd37d39 | 26 | //Hiermee wordt de richting bepaald met behulp van potmeter 1 en wordt de snelheid bepaald aan de hand van de motorValue. |
paulineoonk | 7:df5ad7a84f86 | 27 | if (potMeter1>= 0.5) |
paulineoonk | 7:df5ad7a84f86 | 28 | { //Counterclockwise rotation |
paulineoonk | 7:df5ad7a84f86 | 29 | M1D = 1; //de motor draait tegen de klok in |
Annelotte | 0:85965fd37d39 | 30 | } |
Annelotte | 0:85965fd37d39 | 31 | else |
paulineoonk | 7:df5ad7a84f86 | 32 | { |
paulineoonk | 7:df5ad7a84f86 | 33 | //Clockwise rotation |
paulineoonk | 7:df5ad7a84f86 | 34 | M1D = 0; //de motor draait met de klok mee |
Annelotte | 0:85965fd37d39 | 35 | } |
Annelotte | 0:85965fd37d39 | 36 | if (fabs(motorValue) > 1) |
Annelotte | 0:85965fd37d39 | 37 | { |
paulineoonk | 7:df5ad7a84f86 | 38 | M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) |
Annelotte | 0:85965fd37d39 | 39 | } |
Annelotte | 0:85965fd37d39 | 40 | else |
Annelotte | 0:85965fd37d39 | 41 | { |
paulineoonk | 7:df5ad7a84f86 | 42 | M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 |
Annelotte | 0:85965fd37d39 | 43 | } |
Annelotte | 0:85965fd37d39 | 44 | } |
Annelotte | 0:85965fd37d39 | 45 | |
Annelotte | 0:85965fd37d39 | 46 | float GetReferenceVelocity() |
Annelotte | 0:85965fd37d39 | 47 | { |
Annelotte | 0:85965fd37d39 | 48 | //Returns reference velocity in rad/s |
Annelotte | 0:85965fd37d39 | 49 | //Positieve waarden betekend met de klok mee draaien |
Annelotte | 0:85965fd37d39 | 50 | const float maxVelocity=8.4; //in rad/s |
paulineoonk | 8:432f6d4669e1 | 51 | float refV= potMeter2 * maxVelocity; |
paulineoonk | 7:df5ad7a84f86 | 52 | return refV; |
Annelotte | 0:85965fd37d39 | 53 | } |
Annelotte | 0:85965fd37d39 | 54 | |
paulineoonk | 7:df5ad7a84f86 | 55 | float FeedForwardControl(float refV) //Koppelt de referenceVelocity aan de motorValue |
Annelotte | 0:85965fd37d39 | 56 | { |
Annelotte | 0:85965fd37d39 | 57 | //eenvoudige lineaire feed-forward control |
Annelotte | 0:85965fd37d39 | 58 | const float MotorGain = 8.4; //eenheid: (rad/s) / PWM |
paulineoonk | 7:df5ad7a84f86 | 59 | float motorValue = refV / MotorGain; //bepalen van de fractie van de snelheid t.o.v. max snelheid |
Annelotte | 0:85965fd37d39 | 60 | return motorValue; |
Annelotte | 0:85965fd37d39 | 61 | } |
Annelotte | 0:85965fd37d39 | 62 | |
Annelotte | 0:85965fd37d39 | 63 | void MeasureAndControl(void) |
Annelotte | 0:85965fd37d39 | 64 | { |
Annelotte | 0:85965fd37d39 | 65 | //Deze functie neemt de potmeter positie waar, leidt hier een referentie snelheid vanaf en controleert de motor |
Annelotte | 0:85965fd37d39 | 66 | //met een eenvoudige feed-forward controller. Deze functie wordt aangeroepen met behulp van de ticker Treecko |
paulineoonk | 8:432f6d4669e1 | 67 | float B = motor1.getPosition(); |
paulineoonk | 8:432f6d4669e1 | 68 | float Potmeterwaarde = potMeter1.read(); |
paulineoonk | 8:432f6d4669e1 | 69 | int positie = B%4096; |
paulineoonk | 8:432f6d4669e1 | 70 | if(B <= 4096) |
paulineoonk | 8:432f6d4669e1 | 71 | { |
paulineoonk | 8:432f6d4669e1 | 72 | |
paulineoonk | 7:df5ad7a84f86 | 73 | float refV = GetReferenceVelocity(); |
paulineoonk | 7:df5ad7a84f86 | 74 | float motorValue = FeedForwardControl(refV); |
Annelotte | 0:85965fd37d39 | 75 | SetMotor1(motorValue); |
paulineoonk | 8:432f6d4669e1 | 76 | } |
paulineoonk | 8:432f6d4669e1 | 77 | else |
paulineoonk | 8:432f6d4669e1 | 78 | { |
paulineoonk | 8:432f6d4669e1 | 79 | SetMotor1(0); |
paulineoonk | 8:432f6d4669e1 | 80 | } |
Annelotte | 0:85965fd37d39 | 81 | } |
Annelotte | 0:85965fd37d39 | 82 | |
Annelotte | 0:85965fd37d39 | 83 | int main () |
Annelotte | 0:85965fd37d39 | 84 | { |
paulineoonk | 8:432f6d4669e1 | 85 | Treecko.attach(MeasureAndControl, 0.1); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende |
Annelotte | 4:fbf4307e8aea | 86 | //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. |
paulineoonk | 8:432f6d4669e1 | 87 | |
paulineoonk | 8:432f6d4669e1 | 88 | while(1) { |
paulineoonk | 8:432f6d4669e1 | 89 | wait(0.2); |
paulineoonk | 8:432f6d4669e1 | 90 | pc.baud(115200); |
paulineoonk | 8:432f6d4669e1 | 91 | |
paulineoonk | 8:432f6d4669e1 | 92 | pc.printf("pos: %d, speed %f, potmeter = %f V, p=%i \r\n",motor1.getPosition(), motor1.getSpeed(),potMeter2.read()*3.3,positie); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V |
paulineoonk | 8:432f6d4669e1 | 93 | } |
Annelotte | 3:e1147e50344b | 94 | } |