Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

main.cpp@2:0954eb04bb4c, 2016-10-13 (annotated)

Committer:
s1588141
Date:
Thu Oct 13 11:07:26 2016 +0000
Revision:
2:0954eb04bb4c
Parent:
1:d8bb72c9c019
Child:
3:93f4ab4a7339
Child:
4:8b1df22779a7
Na toevoegen ineffectieve positie uitlezen;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1588141 0:a2db8cc5d5df 1 #include "mbed.h"
s1588141 0:a2db8cc5d5df 2 #include "MODSERIAL.h"
s1588141 0:a2db8cc5d5df 3 #include "QEI.h"
s1588141 2:0954eb04bb4c 4 #include "math.h"
s1588141 2:0954eb04bb4c 5 #include "HIDScope.h"
s1588141 2:0954eb04bb4c 6 #include "Biquad.h"
s1588141 2:0954eb04bb4c 7
s1588141 2:0954eb04bb4c 8 DigitalOut green(LED_GREEN);
s1588141 0:a2db8cc5d5df 9 MODSERIAL pc(USBTX, USBRX);
s1588141 0:a2db8cc5d5df 10 ///////////////////////////
s1588141 0:a2db8cc5d5df 11 // Hardware initialiseren//
s1588141 0:a2db8cc5d5df 12 ///////////////////////////
s1588141 0:a2db8cc5d5df 13 //------------------------------------------------------------
s1588141 2:0954eb04bb4c 14 //--------------------All sensors-----------------------------
s1588141 0:a2db8cc5d5df 15 //------------------------------------------------------------
s1588141 2:0954eb04bb4c 16 //--------------------Encoders--------------------------------
s1588141 0:a2db8cc5d5df 17
s1588141 2:0954eb04bb4c 18 QEI Encoder_L (D11, D10,NC, 32); //D11 is poort A D10 is poort b
s1588141 2:0954eb04bb4c 19 QEI Encoder_R (D13, D12,NC, 32); //D 13 is poort A 12 is poort b
s1588141 2:0954eb04bb4c 20
s1588141 2:0954eb04bb4c 21 double AnglePerPulse = 11.25;
s1588141 2:0954eb04bb4c 22 double Position_L = 0.0;
s1588141 2:0954eb04bb4c 23 double Position_R = 0.0;
s1588141 2:0954eb04bb4c 24 double Previous_Position_L = 0.0;
s1588141 2:0954eb04bb4c 25 double Previous_Position_R = 0.0;
s1588141 0:a2db8cc5d5df 26 //--------------------Analog---------------------------------
s1588141 2:0954eb04bb4c 27 AnalogIn P_M_L(A0); //Motorspeed Left control
s1588141 2:0954eb04bb4c 28 AnalogIn P_M_R(A1); //MotorSpeed Right control
s1588141 0:a2db8cc5d5df 29
s1588141 2:0954eb04bb4c 30 //-------------------Hidscope----------------------------------
s1588141 2:0954eb04bb4c 31 HIDScope scope(2); // Sending two sets of data
s1588141 0:a2db8cc5d5df 32
s1588141 0:a2db8cc5d5df 33 //-------------------Interrupts-------------------------------
s1588141 0:a2db8cc5d5df 34 InterruptIn Dir_L(D0); //Motor Dirrection left
s1588141 0:a2db8cc5d5df 35 InterruptIn Dir_R(D1); //Motor Dirrection Right
s1588141 1:d8bb72c9c019 36 InterruptIn OnOff(SW3); //Motor On/off
s1588141 1:d8bb72c9c019 37
s1588141 2:0954eb04bb4c 38
s1588141 2:0954eb04bb4c 39 //------------------Tickers------------------------------------
s1588141 2:0954eb04bb4c 40
s1588141 2:0954eb04bb4c 41 Ticker Measure; // ticker for data mesuaring
s1588141 0:a2db8cc5d5df 42 //------------------------------------------------------------
s1588141 0:a2db8cc5d5df 43 //--------------------All Motors----------------------------
s1588141 0:a2db8cc5d5df 44 //------------------------------------------------------------
s1588141 0:a2db8cc5d5df 45
s1588141 0:a2db8cc5d5df 46 //-------------------- Motor Links----------------------------
s1588141 0:a2db8cc5d5df 47 DigitalOut M_L_D(D4); //Richting motor links
s1588141 2:0954eb04bb4c 48 PwmOut M_L_S(D5); //Speed motor links
s1588141 0:a2db8cc5d5df 49 DigitalOut M_R_D(D7); //Richting motor Rechts
s1588141 2:0954eb04bb4c 50 PwmOut M_R_S(D6); //Speed motor Rechts
s1588141 0:a2db8cc5d5df 51
s1588141 0:a2db8cc5d5df 52 //--------------------------------------------------------------
s1588141 0:a2db8cc5d5df 53 //-----------------------Functions------------------------------
s1588141 0:a2db8cc5d5df 54 //--------------------------------------------------------------
s1588141 0:a2db8cc5d5df 55
s1588141 0:a2db8cc5d5df 56 //-----------------------Interrupts-----------------------------
s1588141 0:a2db8cc5d5df 57 volatile int Motor_Frequency = 1000;
s1588141 1:d8bb72c9c019 58 volatile bool Active = false;
s1588141 2:0954eb04bb4c 59 volatile float Speed_L= 0.0;
s1588141 2:0954eb04bb4c 60 volatile float Speed_R= 0.0;
s1588141 2:0954eb04bb4c 61
s1588141 0:a2db8cc5d5df 62 void Boot(){
s1588141 0:a2db8cc5d5df 63
s1588141 0:a2db8cc5d5df 64 M_L_S.period(1.0/Motor_Frequency);
s1588141 0:a2db8cc5d5df 65 M_L_D = 1;
s1588141 0:a2db8cc5d5df 66 M_L_S = 0.0;
s1588141 0:a2db8cc5d5df 67 M_R_S.period(1.0/Motor_Frequency);
s1588141 0:a2db8cc5d5df 68 M_R_D = 0;
s1588141 0:a2db8cc5d5df 69 M_R_S= 0.0;
s1588141 2:0954eb04bb4c 70
s1588141 2:0954eb04bb4c 71 Encoder_L.reset();
s1588141 2:0954eb04bb4c 72 Encoder_R.reset();
s1588141 0:a2db8cc5d5df 73 }
s1588141 0:a2db8cc5d5df 74 void Switch_Dirr_L(){ // Switching dirrection left motor (Every motor starts in clockwise dirrection)
s1588141 0:a2db8cc5d5df 75 M_L_D = !M_L_D;
s1588141 0:a2db8cc5d5df 76 }
s1588141 0:a2db8cc5d5df 77
s1588141 0:a2db8cc5d5df 78 void Switch_Dirr_R(){ // Switching dirrection Right motor
s1588141 0:a2db8cc5d5df 79 M_R_D = !M_R_D;
s1588141 0:a2db8cc5d5df 80 }
s1588141 1:d8bb72c9c019 81 void De_Activate(){
s1588141 1:d8bb72c9c019 82 Active = !Active;
s1588141 1:d8bb72c9c019 83 }
s1588141 1:d8bb72c9c019 84
s1588141 1:d8bb72c9c019 85 void Information(){
s1588141 2:0954eb04bb4c 86
s1588141 2:0954eb04bb4c 87 //---------------------Motor Data-----------------------------------------------
s1588141 2:0954eb04bb4c 88 //Position in Radials:
s1588141 2:0954eb04bb4c 89 Previous_Position_L = Position_L;
s1588141 2:0954eb04bb4c 90 Previous_Position_R = Position_R;
s1588141 2:0954eb04bb4c 91 Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
s1588141 2:0954eb04bb4c 92 Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
s1588141 2:0954eb04bb4c 93
s1588141 2:0954eb04bb4c 94 //Speed in RadPers second
s1588141 2:0954eb04bb4c 95 Motor_Speed_L = Previous_Position_L - Position_R
s1588141 2:0954eb04bb4c 96 //---------------------Sending data---------------------------------------------
s1588141 2:0954eb04bb4c 97 scope.set(0,M_L_S); //Sending motor left speed
s1588141 2:0954eb04bb4c 98 scope.set(1,M_R_S); //sending random value
s1588141 2:0954eb04bb4c 99 scope.send();
s1588141 1:d8bb72c9c019 100 }
s1588141 2:0954eb04bb4c 101 //-----------------------Encoders-----------------------------------------------
s1588141 2:0954eb04bb4c 102 int Get_Phase_L(int Phase){
s1588141 2:0954eb04bb4c 103 Phase = Encoder_L.getCurrentState();
s1588141 2:0954eb04bb4c 104 return Phase;
s1588141 2:0954eb04bb4c 105 }
s1588141 0:a2db8cc5d5df 106 int main()
s1588141 0:a2db8cc5d5df 107 {
s1588141 0:a2db8cc5d5df 108 //---------------------Setting constants and system booting--------------------
s1588141 0:a2db8cc5d5df 109 pc.baud(115200);
s1588141 0:a2db8cc5d5df 110 pc.printf("\r\n**BOARD RESET**\r\n");
s1588141 0:a2db8cc5d5df 111 Boot();
s1588141 0:a2db8cc5d5df 112
s1588141 2:0954eb04bb4c 113
s1588141 0:a2db8cc5d5df 114 Dir_L.fall(Switch_Dirr_L);
s1588141 0:a2db8cc5d5df 115 Dir_R.fall(Switch_Dirr_R);
s1588141 1:d8bb72c9c019 116 OnOff.fall(De_Activate);
s1588141 2:0954eb04bb4c 117 Measure.attach(Information, 0.01);
s1588141 0:a2db8cc5d5df 118 while (true) {
s1588141 0:a2db8cc5d5df 119 // control= pc.getc();
s1588141 1:d8bb72c9c019 120 if (Active == true){
s1588141 2:0954eb04bb4c 121 green = 0;
s1588141 2:0954eb04bb4c 122 Speed_L = P_M_L.read();//5.0+0.1;
s1588141 2:0954eb04bb4c 123 M_L_S = Speed_L/5+0.1f;
s1588141 2:0954eb04bb4c 124 Speed_R = P_M_R.read();//5.0+0.1;
s1588141 2:0954eb04bb4c 125 M_R_S = Speed_R/5+0.1f ;
s1588141 1:d8bb72c9c019 126 } else {
s1588141 1:d8bb72c9c019 127 if(Active == false){
s1588141 2:0954eb04bb4c 128 green = 1;
s1588141 1:d8bb72c9c019 129 M_L_S = 0;
s1588141 1:d8bb72c9c019 130 M_R_S = 0;
s1588141 1:d8bb72c9c019 131 }
s1588141 2:0954eb04bb4c 132 }
s1588141 0:a2db8cc5d5df 133 }
s1588141 0:a2db8cc5d5df 134 }