Arnoud Domhof / Mbed 2 deprecated Assignment_PES_controllers

Controlling the motors

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed

main.cpp@5:348cd7d2a094, 2018-10-10 (annotated)

Committer:
AppelSab
Date:
Wed Oct 10 11:56:34 2018 +0000
Revision:
5:348cd7d2a094
Parent:
4:c73ced5d5754
Child:
6:ae2ae12328b7
Eerste merge van potmeter motoren en encoder. Nog niet getest.;

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 directionM1(D4);
Mirjam 0:50c494034326 14 DigitalOut directionM2(D7);
Mirjam 0:50c494034326 15 //D5 is a PWM input for the motor controller and determines the PWM signal
Mirjam 0:50c494034326 16 //that the motor controller gives to Motor 1. Higher PWM, higer average voltage.
Mirjam 0:50c494034326 17 // D6 for motor 2
Mirjam 0:50c494034326 18 FastPWM motor1_pwm(D5);
Mirjam 0:50c494034326 19 FastPWM motor2_pwm(D6);
AppelSab 5:348cd7d2a094 20 // For Encoder reading
AppelSab 5:348cd7d2a094 21 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
AppelSab 5:348cd7d2a094 22 QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
Mirjam 0:50c494034326 23
Mirjam 2:840f7aa50e55 24 // Voor het laten zien van de data op de pc.
Mirjam 2:840f7aa50e55 25 HIDScope scope(2); // Aantal kanalen wat doorgegeven wordt aan de hidscope
Mirjam 2:840f7aa50e55 26 Ticker AInTicker;
AppelSab 5:348cd7d2a094 27 Ticker Encoder;
AppelSab 5:348cd7d2a094 28
AppelSab 5:348cd7d2a094 29 // Declare variables for motor
Mirjam 4:c73ced5d5754 30 float U1;
Mirjam 4:c73ced5d5754 31 float U2;
Mirjam 2:840f7aa50e55 32 float potwaarde1;
Mirjam 2:840f7aa50e55 33 float potwaarde2;
Mirjam 2:840f7aa50e55 34
Mirjam 2:840f7aa50e55 35 void ReadAnalogIn()
Mirjam 2:840f7aa50e55 36 {
Mirjam 4:c73ced5d5754 37 scope.set(0,U1); // Zet de potwaarde in de eerste plot bij de HID scope. Deze wordt automatisch tegen de tijd geplot
Mirjam 4:c73ced5d5754 38 scope.set(1,U2);
Mirjam 2:840f7aa50e55 39 scope.send(); // Zendt de waardes naar de pc
Mirjam 2:840f7aa50e55 40 }
Mirjam 2:840f7aa50e55 41
AppelSab 5:348cd7d2a094 42 void ReadEncoder()
AppelSab 5:348cd7d2a094 43 {
AppelSab 5:348cd7d2a094 44 const float pi = 3.14159265359;
AppelSab 5:348cd7d2a094 45
AppelSab 5:348cd7d2a094 46 // Declare variables
AppelSab 5:348cd7d2a094 47 int counts1;
AppelSab 5:348cd7d2a094 48 int counts2;
AppelSab 5:348cd7d2a094 49 float theta1;
AppelSab 5:348cd7d2a094 50 float theta2;
AppelSab 5:348cd7d2a094 51
AppelSab 5:348cd7d2a094 52 // Get counts
AppelSab 5:348cd7d2a094 53 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 5:348cd7d2a094 54 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 5:348cd7d2a094 55
AppelSab 5:348cd7d2a094 56 // Get angles
AppelSab 5:348cd7d2a094 57 theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 5:348cd7d2a094 58 theta2 = (float(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 5:348cd7d2a094 59
AppelSab 5:348cd7d2a094 60 pc.baud(115200);
AppelSab 5:348cd7d2a094 61 pc.printf("Hoek motor 1 = %0.2f, hoek motor 2 = %0.2f \n \r ", theta1, theta2);
AppelSab 5:348cd7d2a094 62 }
AppelSab 5:348cd7d2a094 63
AppelSab 5:348cd7d2a094 64
AppelSab 5:348cd7d2a094 65
Mirjam 0:50c494034326 66 int main(void)
Mirjam 0:50c494034326 67 {
AppelSab 5:348cd7d2a094 68 motor1_pwm.period_ms(60); // Period is 60 ms
AppelSab 5:348cd7d2a094 69 AInTicker.attach(&ReadAnalogIn,0.01f); // Elke 0.01 sec. Lees de analoge waarde
AppelSab 5:348cd7d2a094 70 Encoder.attach(&ReadEncoder, 1.0f); // Lees elke 1 sec de encoder uit
AppelSab 5:348cd7d2a094 71
Mirjam 0:50c494034326 72 while(true){
Mirjam 4:c73ced5d5754 73 potwaarde1 = potmeter1.read(); // Lees de potwaardes uit. Tussen 0 en 1
Mirjam 2:840f7aa50e55 74 potwaarde2 = potmeter2.read();
Mirjam 4:c73ced5d5754 75
Mirjam 4:c73ced5d5754 76 U1 = potwaarde1*2 -1; // Scale van -1 tot 1 ipv. 0 tot 1
Mirjam 4:c73ced5d5754 77 U2 = potwaarde2*2 -1;
Mirjam 4:c73ced5d5754 78
Mirjam 4:c73ced5d5754 79 // Gebruik de absolute waarde van de scaled U waardes als input voor de motor.
Mirjam 4:c73ced5d5754 80 // Negatieve waardes kunnen niet naar de motor gestuurd worden.
Mirjam 4:c73ced5d5754 81 motor1_pwm.write(fabs(U1));
Mirjam 4:c73ced5d5754 82 motor2_pwm.write(fabs(U2));
Mirjam 4:c73ced5d5754 83
Mirjam 4:c73ced5d5754 84 directionM1 = U1 > 0.0f; //either true or false
Mirjam 4:c73ced5d5754 85 directionM2 = U2 > 0.0f;
Mirjam 4:c73ced5d5754 86
AppelSab 5:348cd7d2a094 87 wait(0.002f);
AppelSab 5:348cd7d2a094 88 }
Mirjam 0:50c494034326 89
Mirjam 0:50c494034326 90 }