Arnoud Domhof / Mbed 2 deprecated Assignment_PES_controllers

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed

main.cpp@7:2440100fe04c, 2018-10-16 (annotated)

Committer:
AppelSab
Date:
Tue Oct 16 09:13:18 2018 +0000
Revision:
7:2440100fe04c
Parent:
6:ae2ae12328b7
Child:
8:61a3a5fb4c37
Hallo;

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);
Mirjam 0:50c494034326 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
Mirjam 0:50c494034326 13 DigitalOut directionM2(D7);
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;
AppelSab 5:348cd7d2a094 24
AppelSab 5:348cd7d2a094 25 // Declare variables for motor
AppelSab 7:2440100fe04c 26 float pos;
Mirjam 2:840f7aa50e55 27 float potwaarde1;
Mirjam 2:840f7aa50e55 28 float potwaarde2;
AppelSab 7:2440100fe04c 29 const float pi = 3.14159265359;
AppelSab 7:2440100fe04c 30 int counts1;
AppelSab 7:2440100fe04c 31 float theta1;
AppelSab 7:2440100fe04c 32
Mirjam 2:840f7aa50e55 33
Mirjam 2:840f7aa50e55 34 void ReadAnalogIn()
Mirjam 2:840f7aa50e55 35 {
AppelSab 7:2440100fe04c 36 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 37 scope.send(); // Zendt de waardes naar de pc
Mirjam 2:840f7aa50e55 38 }
Mirjam 2:840f7aa50e55 39
AppelSab 7:2440100fe04c 40 float ReadEncoder()
AppelSab 5:348cd7d2a094 41 {
AppelSab 5:348cd7d2a094 42 // Get counts
AppelSab 5:348cd7d2a094 43 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 5:348cd7d2a094 44
AppelSab 5:348cd7d2a094 45 // Get angles
AppelSab 5:348cd7d2a094 46 theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 5:348cd7d2a094 47
AppelSab 5:348cd7d2a094 48 pc.baud(115200);
AppelSab 7:2440100fe04c 49 pc.printf("Hoek motor 1 = %0.2f rad \n \r", theta1);
AppelSab 7:2440100fe04c 50 return theta1;
AppelSab 5:348cd7d2a094 51 }
AppelSab 5:348cd7d2a094 52
AppelSab 7:2440100fe04c 53 float P_controller(float error)
AppelSab 7:2440100fe04c 54 {
AppelSab 7:2440100fe04c 55 float Kp = 2.0;
AppelSab 7:2440100fe04c 56 float U1 = Kp*error;
AppelSab 7:2440100fe04c 57 return U1;
AppelSab 7:2440100fe04c 58 }
AppelSab 5:348cd7d2a094 59
Mirjam 0:50c494034326 60 int main(void)
Mirjam 0:50c494034326 61 {
AppelSab 7:2440100fe04c 62 motor1_pwm.period_us(60); // Period is 60 microseconde
AppelSab 5:348cd7d2a094 63 AInTicker.attach(&ReadAnalogIn,0.01f); // Elke 0.01 sec. Lees de analoge waarde
AppelSab 5:348cd7d2a094 64
Mirjam 0:50c494034326 65 while(true){
Mirjam 4:c73ced5d5754 66 potwaarde1 = potmeter1.read(); // Lees de potwaardes uit. Tussen 0 en 1
Mirjam 2:840f7aa50e55 67 potwaarde2 = potmeter2.read();
AppelSab 7:2440100fe04c 68
AppelSab 7:2440100fe04c 69
AppelSab 7:2440100fe04c 70 pos = (potwaarde1*2 -1) * 2*pi; // Scale van -1 tot 1 ipv. 0 tot 1
AppelSab 7:2440100fe04c 71 theta1 = ReadEncoder();
AppelSab 7:2440100fe04c 72 float error = pos - theta1;
AppelSab 7:2440100fe04c 73 float U1 = P_controller(error);
Mirjam 4:c73ced5d5754 74
Mirjam 4:c73ced5d5754 75 // Gebruik de absolute waarde van de scaled U waardes als input voor de motor.
Mirjam 4:c73ced5d5754 76 // Negatieve waardes kunnen niet naar de motor gestuurd worden.
Mirjam 4:c73ced5d5754 77 motor1_pwm.write(fabs(U1));
Mirjam 4:c73ced5d5754 78
AppelSab 7:2440100fe04c 79 float directionM1 = U1 > 0.0f; //either true or false
Mirjam 4:c73ced5d5754 80
AppelSab 5:348cd7d2a094 81 wait(0.002f);
AppelSab 5:348cd7d2a094 82 }
Mirjam 0:50c494034326 83
Mirjam 0:50c494034326 84 }