ProjectGroep23 / Mbed 2 deprecated PI_controller_verbeteringen

PI controller met potmeters om de Kp en Ki in te stellen

Dependencies:   MODSERIAL QEI mbed-dsp mbed

Fork of Motorcode by ProjectGroep23

main.cpp@9:466dff9ae128, 2018-10-05 (annotated)

Committer:
rachieldb
Date:
Fri Oct 05 12:51:06 2018 +0000
Revision:
9:466dff9ae128
Parent:
8:9b517db94f49
Child:
10:61a7cb3b3aa3
Encoder angle toegevoegd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
SimonRez 6:1ee8795b7578 3 #include "QEI.h"
rachieldb 9:466dff9ae128 4 #include "math.h"
rachieldb 9:466dff9ae128 5 #define _USE_MATH_DEFINES
rachieldb 9:466dff9ae128 6 # define M_PI 3.14159265358979323846 /* pi */
SimonRez 2:52b3c0b95388 7
SimonRez 2:52b3c0b95388 8 DigitalOut ledr(LED_RED);
SimonRez 2:52b3c0b95388 9 DigitalOut ledg(LED_GREEN);
SimonRez 2:52b3c0b95388 10 DigitalOut ledb(LED_BLUE);
SimonRez 4:651d06e860e7 11
rachieldb 9:466dff9ae128 12 PwmOut motor1_pwm(D5);
rachieldb 9:466dff9ae128 13 PwmOut motor2_pwm(D6);
rachieldb 9:466dff9ae128 14 DigitalOut motor1_dir(D4);
rachieldb 9:466dff9ae128 15 DigitalOut motor2_dir(D7);
SimonRez 6:1ee8795b7578 16 AnalogIn pot1(A0);
rachieldb 9:466dff9ae128 17 AnalogIn pot2(A1);
SimonRez 6:1ee8795b7578 18
rachieldb 9:466dff9ae128 19 QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
rachieldb 9:466dff9ae128 20 QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);
SimonRez 5:f07bafaf11d7 21
SimonRez 4:651d06e860e7 22 InterruptIn sw2(SW2);
SimonRez 4:651d06e860e7 23 InterruptIn sw3(SW3);
SimonRez 2:52b3c0b95388 24
vsluiter 0:c8f15874531b 25 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 26
rachieldb 9:466dff9ae128 27 Ticker sample_timer;
rachieldb 9:466dff9ae128 28
rachieldb 9:466dff9ae128 29 const float sample_time = 0.05; //s
rachieldb 9:466dff9ae128 30 const float sample_frequency = 200; //hz
rachieldb 9:466dff9ae128 31
rachieldb 9:466dff9ae128 32 double countstoangle(int counts){
rachieldb 9:466dff9ae128 33 double angle;
rachieldb 9:466dff9ae128 34 int counts_abs = abs(counts);
rachieldb 9:466dff9ae128 35 if(counts_abs >= 8400){
rachieldb 9:466dff9ae128 36 int partial_counts = counts_abs % 8400;
rachieldb 9:466dff9ae128 37 angle = (2 * M_PI * partial_counts)/8400;
rachieldb 9:466dff9ae128 38 return angle;
rachieldb 9:466dff9ae128 39 }else{
rachieldb 9:466dff9ae128 40 angle = (2 * M_PI * counts_abs)/8400;
rachieldb 9:466dff9ae128 41 return angle;
rachieldb 9:466dff9ae128 42 }
rachieldb 9:466dff9ae128 43 }
SimonRez 4:651d06e860e7 44
vsluiter 0:c8f15874531b 45 int main()
vsluiter 0:c8f15874531b 46 {
SimonRez 2:52b3c0b95388 47 ledr = 1;
SimonRez 2:52b3c0b95388 48 ledg = 1;
SimonRez 2:52b3c0b95388 49 ledb = 1;
SimonRez 2:52b3c0b95388 50
SimonRez 4:651d06e860e7 51 pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
SimonRez 2:52b3c0b95388 52
rachieldb 9:466dff9ae128 53 motor1_pwm.period_us(60);
rachieldb 9:466dff9ae128 54 motor1_pwm = 0;
rachieldb 9:466dff9ae128 55 motor2_pwm.period_us(60);
rachieldb 9:466dff9ae128 56 motor2_pwm = 0;
SimonRez 5:f07bafaf11d7 57
SimonRez 2:52b3c0b95388 58 while (true)
SimonRez 2:52b3c0b95388 59 {
rachieldb 8:9b517db94f49 60 float pot1_float = pot1.read();
rachieldb 8:9b517db94f49 61 float pot1_motor = (pot1_float * 2.0f) - 1.0f;
rachieldb 8:9b517db94f49 62 int mot1_direction = pot1_motor >= 0;
rachieldb 9:466dff9ae128 63 motor1_pwm.write(fabs(pot1_motor));
rachieldb 9:466dff9ae128 64 motor1_dir = !mot1_direction;
rachieldb 9:466dff9ae128 65
rachieldb 9:466dff9ae128 66 float pot2_float = pot2.read();
rachieldb 9:466dff9ae128 67 float pot2_motor = (pot2_float * 2.0f) - 1.0f;
rachieldb 9:466dff9ae128 68 int mot2_direction = pot2_motor >= 0;
rachieldb 9:466dff9ae128 69 motor2_pwm.write(fabs(pot2_motor));
rachieldb 9:466dff9ae128 70 motor2_dir = mot2_direction;
rachieldb 9:466dff9ae128 71
rachieldb 9:466dff9ae128 72 double motor1_ang = countstoangle(motor1_enc.getPulses());
rachieldb 9:466dff9ae128 73 double motor2_ang = countstoangle(motor2_enc.getPulses());
rachieldb 9:466dff9ae128 74
rachieldb 9:466dff9ae128 75 pc.printf("Angle motor 1: %f pi rad, angle motor 2: %f pi rad.\r\n", motor1_ang/(M_PI), motor2_ang/(M_PI));
rachieldb 9:466dff9ae128 76
SimonRez 5:f07bafaf11d7 77 wait(0.2f);
vsluiter 0:c8f15874531b 78 }
vsluiter 0:c8f15874531b 79 }
SimonRez 2:52b3c0b95388 80
SimonRez 2:52b3c0b95388 81