Arnoud Domhof / Mbed 2 deprecated Assignment_PES_controllers

Controlling the motors

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed

main.cpp@8:61a3a5fb4c37, 2018-10-16 (annotated)

Committer:
arnouddomhof
Date:
Tue Oct 16 11:43:21 2018 +0000
Revision:
8:61a3a5fb4c37
Parent:
7:2440100fe04c 
Een poging tot het werkend maken van de p-controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mirjam 0:50c494034326 1 #include "mbed.h"
Mirjam 0:50c494034326 2 #include "FastPWM.h"
Mirjam 0:50c494034326 3 #include "MODSERIAL.h"
Mirjam 0:50c494034326 4 #include "HIDScope.h"
AppelSab 5:348cd7d2a094 5 #include "QEI.h"
Mirjam 0:50c494034326 6
Mirjam 0:50c494034326 7 AnalogIn potmeter1(PTC10);
arnouddomhof 8:61a3a5fb4c37 8 //AnalogIn potmeter2(PTC11);
Mirjam 0:50c494034326 9 MODSERIAL pc(USBTX, USBRX);
Mirjam 0:50c494034326 10 //D4 is a digital input for the microcontroller, so should be an digitalOut
Mirjam 0:50c494034326 11 //from the K64F. It will tell the motor shiel to let Motor1 turn clockwise
Mirjam 0:50c494034326 12 //of count clockwise (CW of CCW). D4 for motor 2
arnouddomhof 8:61a3a5fb4c37 13 DigitalOut directionM1(D4);
Mirjam 0:50c494034326 14 //D5 is a PWM input for the motor controller and determines the PWM signal
Mirjam 0:50c494034326 15 //that the motor controller gives to Motor 1. Higher PWM, higer average voltage.
Mirjam 0:50c494034326 16 // D6 for motor 2
Mirjam 0:50c494034326 17 FastPWM motor1_pwm(D5);
AppelSab 5:348cd7d2a094 18 // For Encoder reading
AppelSab 5:348cd7d2a094 19 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
Mirjam 0:50c494034326 20
Mirjam 2:840f7aa50e55 21 // Voor het laten zien van de data op de pc.
AppelSab 7:2440100fe04c 22 HIDScope scope(1); // Aantal kanalen wat doorgegeven wordt aan de hidscope
Mirjam 2:840f7aa50e55 23 Ticker AInTicker;
arnouddomhof 8:61a3a5fb4c37 24 Ticker Waardesmotor;
AppelSab 5:348cd7d2a094 25
AppelSab 5:348cd7d2a094 26 // Declare variables for motor
AppelSab 7:2440100fe04c 27 float pos;
Mirjam 2:840f7aa50e55 28 float potwaarde1;
arnouddomhof 8:61a3a5fb4c37 29 //float potwaarde2;
AppelSab 7:2440100fe04c 30 const float pi = 3.14159265359;
AppelSab 7:2440100fe04c 31 int counts1;
AppelSab 7:2440100fe04c 32 float theta1;
arnouddomhof 8:61a3a5fb4c37 33 float error1;
arnouddomhof 8:61a3a5fb4c37 34 float U1;
arnouddomhof 8:61a3a5fb4c37 35
AppelSab 7:2440100fe04c 36
Mirjam 2:840f7aa50e55 37
Mirjam 2:840f7aa50e55 38 void ReadAnalogIn()
Mirjam 2:840f7aa50e55 39 {
AppelSab 7:2440100fe04c 40 scope.set(0,pos); // Zet de potwaarde in de eerste plot bij de HID scope. Deze wordt automatisch tegen de tijd geplot
Mirjam 2:840f7aa50e55 41 scope.send(); // Zendt de waardes naar de pc
Mirjam 2:840f7aa50e55 42 }
Mirjam 2:840f7aa50e55 43
AppelSab 7:2440100fe04c 44 float ReadEncoder()
AppelSab 5:348cd7d2a094 45 {
AppelSab 5:348cd7d2a094 46 // Get counts
AppelSab 5:348cd7d2a094 47 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 5:348cd7d2a094 48
AppelSab 5:348cd7d2a094 49 // Get angles
AppelSab 5:348cd7d2a094 50 theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 5:348cd7d2a094 51
arnouddomhof 8:61a3a5fb4c37 52 //pc.printf("Hoek motor 1 = %0.2f rad \n \r", theta1);
AppelSab 7:2440100fe04c 53 return theta1;
AppelSab 5:348cd7d2a094 54 }
AppelSab 5:348cd7d2a094 55
arnouddomhof 8:61a3a5fb4c37 56 /**
AppelSab 7:2440100fe04c 57 float P_controller(float error)
AppelSab 7:2440100fe04c 58 {
AppelSab 7:2440100fe04c 59 float Kp = 2.0;
AppelSab 7:2440100fe04c 60 float U1 = Kp*error;
AppelSab 7:2440100fe04c 61 return U1;
AppelSab 7:2440100fe04c 62 }
arnouddomhof 8:61a3a5fb4c37 63 */
arnouddomhof 8:61a3a5fb4c37 64
arnouddomhof 8:61a3a5fb4c37 65 void Waardes()
arnouddomhof 8:61a3a5fb4c37 66 {
arnouddomhof 8:61a3a5fb4c37 67 pc.printf("Hoek motor 1 = %0.2f rad, hoek potmeter = %0.2f rad, error = %0.2f rad, U1 = %0.2f \n \r", theta1, pos, error1,U1);
arnouddomhof 8:61a3a5fb4c37 68 }
arnouddomhof 8:61a3a5fb4c37 69
AppelSab 5:348cd7d2a094 70
Mirjam 0:50c494034326 71 int main(void)
Mirjam 0:50c494034326 72 {
arnouddomhof 8:61a3a5fb4c37 73 pc.baud(115200);
AppelSab 7:2440100fe04c 74 motor1_pwm.period_us(60); // Period is 60 microseconde
arnouddomhof 8:61a3a5fb4c37 75 AInTicker.attach(&ReadAnalogIn,0.001f); // Elke 0.001 sec. Lees de analoge waarde
arnouddomhof 8:61a3a5fb4c37 76 Waardesmotor.attach(Waardes,1); // Elke 1 sec wordt de waarde van motor 1 geplot
AppelSab 5:348cd7d2a094 77
Mirjam 0:50c494034326 78 while(true){
Mirjam 4:c73ced5d5754 79 potwaarde1 = potmeter1.read(); // Lees de potwaardes uit. Tussen 0 en 1
arnouddomhof 8:61a3a5fb4c37 80 //potwaarde2 = potmeter2.read();
AppelSab 7:2440100fe04c 81
AppelSab 7:2440100fe04c 82
arnouddomhof 8:61a3a5fb4c37 83 pos = (potwaarde1*2 - 1) * 2*pi; // Scale van -1 tot 1 ipv. 0 tot 1
AppelSab 7:2440100fe04c 84 theta1 = ReadEncoder();
arnouddomhof 8:61a3a5fb4c37 85 error1 = pos - theta1;
arnouddomhof 8:61a3a5fb4c37 86 float Kp = 1;
arnouddomhof 8:61a3a5fb4c37 87 U1 = Kp*float(error1)/6.28;
arnouddomhof 8:61a3a5fb4c37 88
arnouddomhof 8:61a3a5fb4c37 89 //float U1 = P_controller(error);
Mirjam 4:c73ced5d5754 90
Mirjam 4:c73ced5d5754 91 // Gebruik de absolute waarde van de scaled U waardes als input voor de motor.
Mirjam 4:c73ced5d5754 92 // Negatieve waardes kunnen niet naar de motor gestuurd worden.
Mirjam 4:c73ced5d5754 93 motor1_pwm.write(fabs(U1));
Mirjam 4:c73ced5d5754 94
arnouddomhof 8:61a3a5fb4c37 95 directionM1 = U1 > 0.0f; //either true or false
Mirjam 4:c73ced5d5754 96
AppelSab 5:348cd7d2a094 97 wait(0.002f);
AppelSab 5:348cd7d2a094 98 }
Mirjam 0:50c494034326 99
Mirjam 0:50c494034326 100 }