Basis aansturing projectgroep 3
Dependencies: Biquad HIDScope MODSERIAL QEI mbed
main.cpp
- Committer:
- s1588141
- Date:
- 2016-10-14
- Revision:
- 3:93f4ab4a7339
- Parent:
- 2:0954eb04bb4c
File content as of revision 3:93f4ab4a7339:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "math.h" #include "HIDScope.h" #include "Biquad.h" DigitalOut green(LED_GREEN); MODSERIAL pc(USBTX, USBRX); /////////////////////////// // Hardware initialiseren// /////////////////////////// //------------------------------------------------------------ //--------------------All sensors----------------------------- //------------------------------------------------------------ //--------------------Encoders-------------------------------- QEI Encoder_L (D11, D10,NC, 32); //D11 is poort A D10 is poort b QEI Encoder_R (D13, D12,NC, 32); //D 13 is poort A 12 is poort b double AnglePerPulse = 11.25; double Position_L = 0.0; double Position_R = 0.0; double Previous_Position_L = 0.0; double Previous_Position_R = 0.0; //--------------------Analog--------------------------------- AnalogIn P_M_L(A0); //Motorspeed Left control AnalogIn P_M_R(A1); //MotorSpeed Right control //-------------------Hidscope---------------------------------- HIDScope scope(2); // Sending two sets of data //-------------------Interrupts------------------------------- InterruptIn Dir_L(D0); //Motor Dirrection left InterruptIn Dir_R(D1); //Motor Dirrection Right InterruptIn OnOff(SW3); //Motor On/off //------------------Tickers------------------------------------ Ticker Measure; // ticker for data mesuaring //------------------------------------------------------------ //--------------------All Motors---------------------------- //------------------------------------------------------------ //-------------------- Motor Links---------------------------- DigitalOut M_L_D(D4); //Richting motor links PwmOut M_L_S(D5); //Speed motor links DigitalOut M_R_D(D7); //Richting motor Rechts PwmOut M_R_S(D6); //Speed motor Rechts //-------------------------------------------------------------- //-----------------------Functions------------------------------ //-------------------------------------------------------------- //-----------------------Interrupts----------------------------- volatile int Motor_Frequency = 1000; volatile bool Active = false; volatile float Speed_L= 0.0; volatile float Speed_R= 0.0; void Boot(){ M_L_S.period(1.0/Motor_Frequency); M_L_D = 1; M_L_S = 0.0; M_R_S.period(1.0/Motor_Frequency); M_R_D = 0; M_R_S= 0.0; Encoder_L.reset(); Encoder_R.reset(); } void Switch_Dirr_L(){ // Switching dirrection left motor (Every motor starts in clockwise dirrection) M_L_D = !M_L_D; } void Switch_Dirr_R(){ // Switching dirrection Right motor M_R_D = !M_R_D; } void De_Activate(){ Active = !Active; } void Information(){ //---------------------Motor Data----------------------------------------------- //Position in Radials: Previous_Position_L = Position_L; Previous_Position_R = Position_R; Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L; Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R; //Speed in RadPers second //---------------------Sending data--------------------------------------------- scope.set(0,M_L_S); //Sending motor left speed scope.set(1,M_R_S); //sending random value scope.send(); } //-----------------------Encoders----------------------------------------------- int Get_Phase_L(){ int Phase = Encoder_L.getCurrentState(); return Phase; } int main() { //---------------------Setting constants and system booting-------------------- pc.baud(115200); pc.printf("\r\n**BOARD RESET**\r\n"); Boot(); Dir_L.fall(Switch_Dirr_L); Dir_R.fall(Switch_Dirr_R); OnOff.fall(De_Activate); Measure.attach(Information, 0.01); while (true) { // control= pc.getc(); if (Active == true){ green = 0; Speed_L = P_M_L.read();//5.0+0.1; M_L_S = Speed_L/5+0.1f; Speed_R = P_M_R.read();//5.0+0.1; M_R_S = Speed_R/5+0.1f ; } else { if(Active == false){ green = 1; M_L_S = 0; M_R_S = 0; } } } }