Marlies Weggemans / Mbed 2 deprecated Biorobotics_g15_gezamelijk710

met comments 710

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM demomode

main.cpp@28:5250ef542ffd, 2019-10-07 (annotated)

Committer:
MarliesWeggemans
Date:
Mon Oct 07 09:17:55 2019 +0000
Revision:
28:5250ef542ffd
Parent:
25:00488a279da1 
main incl. comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
boydmartherus 16:0ff43ed4d259 2 #include "MODSERIAL.h"
boydmartherus 17:3751c2c685ca 3 #include "FastPWM.h"
boydmartherus 17:3751c2c685ca 4 #include "QEI.h"
Mortimerz 24:0b5b7ec374e6 5 #include "math.h"
boydmartherus 7:d5e07647dfea 6
boydmartherus 7:d5e07647dfea 7 Serial pc(USBTX,USBRX);
boydmartherus 7:d5e07647dfea 8
boydmartherus 17:3751c2c685ca 9 // functies aan PINs toevoegen
Mortimerz 19:dd9a61699449 10 DigitalOut led1(D2);
boydmartherus 9:cdcf30051100 11 FastPWM led(PTA1);
Mortimerz 23:cd0a4eb31a90 12 FastPWM motor1(D6);
Mortimerz 23:cd0a4eb31a90 13 DigitalOut motor1_dir(D7);
Mortimerz 23:cd0a4eb31a90 14 FastPWM motor2(D5);
Mortimerz 23:cd0a4eb31a90 15 DigitalOut motor2_dir(D4);
Mortimerz 23:cd0a4eb31a90 16 AnalogIn pot1(PTB2);
Mortimerz 23:cd0a4eb31a90 17 AnalogIn pot2(PTB3);
Mortimerz 23:cd0a4eb31a90 18 InterruptIn button1(PTB10);
Mortimerz 23:cd0a4eb31a90 19 InterruptIn button2(PTB11);
boydmartherus 9:cdcf30051100 20
boydmartherus 17:3751c2c685ca 21 // timer opzetten = ticker
Mortimerz 19:dd9a61699449 22 Ticker ticker;
boydmartherus 17:3751c2c685ca 23
boydmartherus 17:3751c2c685ca 24 // opzetten van hoekbepaling
MarliesWeggemans 28:5250ef542ffd 25 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING); // Pulsesperrevolution=64
MarliesWeggemans 28:5250ef542ffd 26 QEI encoder_2(D11,D10,NC,64,QEI::X4_ENCODING);
MarliesWeggemans 4:3e9a8f5d9a1a 27
Mortimerz 24:0b5b7ec374e6 28 //Global values
Mortimerz 23:cd0a4eb31a90 29 int motordir1 = 1;
Mortimerz 23:cd0a4eb31a90 30 int motordir2 = 1;
MarliesWeggemans 28:5250ef542ffd 31 double Kp = 17.5; //proportional gain = tuning parameter
Mortimerz 24:0b5b7ec374e6 32
boydmartherus 17:3751c2c685ca 33 // aparte functies
Mortimerz 22:8340aa6676d7 34 //functie motor aansturen met pot
Mortimerz 24:0b5b7ec374e6 35
Mortimerz 24:0b5b7ec374e6 36
Mortimerz 24:0b5b7ec374e6 37
Mortimerz 24:0b5b7ec374e6 38 void motor_position(double y_des)
boydmartherus 17:3751c2c685ca 39 {
Mortimerz 22:8340aa6676d7 40
Mortimerz 24:0b5b7ec374e6 41 double y_pos = encoder_2.getPulses();
Mortimerz 24:0b5b7ec374e6 42 double y_poscorrect = y_pos/8400.0;
Mortimerz 24:0b5b7ec374e6 43 double error = y_des-y_poscorrect;
Mortimerz 24:0b5b7ec374e6 44 if (error >=0) motor1_dir=1;
Mortimerz 24:0b5b7ec374e6 45 else motor1_dir=0;
Mortimerz 24:0b5b7ec374e6 46 if (fabs(error)>1) motor1 = 1;
Mortimerz 24:0b5b7ec374e6 47 else motor1 = fabs(error);
boydmartherus 17:3751c2c685ca 48 }
Mortimerz 22:8340aa6676d7 49
boydmartherus 17:3751c2c685ca 50
boydmartherus 17:3751c2c685ca 51 void main_loop()
Mortimerz 19:dd9a61699449 52 {
Mortimerz 25:00488a279da1 53 double potpos = pot1.read();
Mortimerz 25:00488a279da1 54 motor_position(potpos);
Mortimerz 19:dd9a61699449 55
boydmartherus 17:3751c2c685ca 56 }
boydmartherus 18:3de4412d8b63 57
boydmartherus 10:9101c7a4f219 58 int main()
boydmartherus 10:9101c7a4f219 59 {
Mortimerz 23:cd0a4eb31a90 60 motor1_dir.write(motordir1);
Mortimerz 23:cd0a4eb31a90 61 motor2_dir.write(motordir2);
Mortimerz 25:00488a279da1 62 ticker.attach(&main_loop, 0.001f);
Mortimerz 19:dd9a61699449 63 while(true){
Mortimerz 23:cd0a4eb31a90 64 pc.printf("pot1(%f),enc1 is (%d),pot2(%f),enc2 is (%d)\r\n",pot1.read(),encoder.getPulses(),pot2.read(),encoder_2.getPulses());
Mortimerz 19:dd9a61699449 65 wait(0.1f);
Mortimerz 19:dd9a61699449 66 };
boydmartherus 10:9101c7a4f219 67 }