Projectgroep 20 Biorobotics / Mbed 2 deprecated motor_met_potmeters_me_aanuit_sneller

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 Projectgroep 20 Biorobotics

main.cpp@9:9a356a676a7c, 2017-10-09 (annotated)

Committer:
paulineoonk
Date:
Mon Oct 09 15:04:23 2017 +0000
Revision:
9:9a356a676a7c
Parent:
8:432f6d4669e1 
rotzooitje, P controller probeersel;

Who changed what in which revision?

UserRevisionLine numberNew 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 9:9a356a676a7c 67 float Huidigepositie = motor1.getPosition();
paulineoonk 9:9a356a676a7c 68 float Potmeterwaarde = potMeter2.read();
paulineoonk 9:9a356a676a7c 69
paulineoonk 9:9a356a676a7c 70 if(Huidigepositie <= 4096)
paulineoonk 8:432f6d4669e1 71 {
paulineoonk 9:9a356a676a7c 72 int maxwaarde = 4096;
paulineoonk 9:9a356a676a7c 73 float Potrefposition = Potmeterwaarde*maxwaarde;
paulineoonk 9:9a356a676a7c 74 float kp = 50;
paulineoonk 9:9a356a676a7c 75 float motorValue = kp*(Potrefposition - Huidigepositie);
paulineoonk 9:9a356a676a7c 76 //float refV = GetReferenceVelocity();
paulineoonk 9:9a356a676a7c 77 //float motorValue = FeedForwardControl(refV);
Annelotte 0:85965fd37d39 78 SetMotor1(motorValue);
paulineoonk 8:432f6d4669e1 79 }
paulineoonk 9:9a356a676a7c 80 /* else
paulineoonk 8:432f6d4669e1 81 {
paulineoonk 8:432f6d4669e1 82 SetMotor1(0);
paulineoonk 8:432f6d4669e1 83 }
paulineoonk 9:9a356a676a7c 84 */
Annelotte 0:85965fd37d39 85 }
Annelotte 0:85965fd37d39 86
Annelotte 0:85965fd37d39 87 int main ()
Annelotte 0:85965fd37d39 88 {
paulineoonk 8:432f6d4669e1 89 Treecko.attach(MeasureAndControl, 0.1); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
Annelotte 4:fbf4307e8aea 90 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
paulineoonk 8:432f6d4669e1 91
paulineoonk 8:432f6d4669e1 92 while(1) {
paulineoonk 8:432f6d4669e1 93 wait(0.2);
paulineoonk 8:432f6d4669e1 94 pc.baud(115200);
paulineoonk 9:9a356a676a7c 95 float B = motor1.getPosition();
paulineoonk 9:9a356a676a7c 96 float Potmeterwaarde = potMeter2.read();
paulineoonk 9:9a356a676a7c 97 // float positie = B%4096;
paulineoonk 9:9a356a676a7c 98 pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
paulineoonk 8:432f6d4669e1 99 }
Annelotte 3:e1147e50344b 100 }