Basis aansturing projectgroep 3
Dependencies: Biquad HIDScope MODSERIAL QEI mbed
main.cpp
- Committer:
- s1588141
- Date:
- 2016-11-07
- Revision:
- 9:e764cb50d343
- Parent:
- 8:656b0c493a45
File content as of revision 9:e764cb50d343:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "math.h" #include "HIDScope.h" #include "BiQuad.h" /////////////////////////// ////////Constantes///////// /////////////////////////// double const pi = 3.14159265359; /////////////////////////// // Initialise hardware///// /////////////////////////// MODSERIAL pc(USBTX, USBRX); //----------------------------------------------------------- //--------------------All lights----------------------------- //----------------------------------------------------------- DigitalOut green(LED_GREEN); DigitalOut red(LED_RED); //------------------------------------------------------------ //--------------------All sensors----------------------------- //------------------------------------------------------------ //--------------------Encoders-------------------------------- //Variables volatile double Signal = 0.0; const int SampleFrequency = 400; const double AnglePerPulse = 2*pi/4200; //Soms doet dit een beetje vaag, dus dit moet wel af en toe uitgetest worden volatile double Position_L = 0.0; //The position of the left motor in radiants volatile double Position_R = 0.0; //The position of the right motor in radiants volatile double Previous_Position_L =0.0; //The previous position of the left motor in radiants volatile double Previous_Position_R = 0.0; //The previous position of the right motor in radiants volatile double Speed_L = 0.0; //The speed of the left motor volatile double Speed_R = 0.0; //The speed of the right motor //Defining Pins QEI Encoder_L (D11, D10,NC, 64); //D11 is poort A D10 is poort b QEI Encoder_R (D13, D12,NC, 64); //D 13 is poort A 12 is poort b //--------------------AnalogInput--------------------------------- AnalogIn Left(A0); //Motorspeed control Left AnalogIn Right(A1); //MotorSpeed control Right AnalogIn Change(A2);//EMG signal for other controls double EMG_L; double EMG_R; double EMG_Change; //-------------------------------------------------------------- //--------------------All Motors-------------------------------- //-------------------------------------------------------------- volatile int movement = 1,direction_L =1, direction_R =-1; //determining the direction of the motors DigitalOut M_L_D(D4); //Richting motor links-> //while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive PwmOut M_L_S(D5); //Speed motor links DigitalOut M_R_D(D7); //Richting motor Rechts PwmOut M_R_S(D6); //Speed motor Rechts const int Motor_Frequency = 1000; DigitalOut Grap(D9); //Graple active or not; //----------------------Lights ---------------------------------4 DigitalOut translation(D0); DigitalOut grip1(D1); DigitalOut rotation(D2); DigitalOut grip2(D3); //-------------------------------------------------------------- //-----------------------Functions------------------------------ //-------------------------------------------------------------- //-----------------------on of sitch of the sytem----------------------------- AnalogIn Active(A5); //--------------------------------Sampling------------------------------------------ Ticker Tick_Sample;// ticker for data sampling bool Sample_Flag = false; void Set_Sample_Flag(){ Sample_Flag = true; } void Sample(){ //This function reads out the position and the speed of the motors in radiants //Aand stores them in the values Speed and Position //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 Speed_L = (Position_L-Previous_Position_L)*SampleFrequency; Speed_R = (Position_R-Previous_Position_R)*SampleFrequency; Encoder_L.reset(); Encoder_R.reset(); } bool Controller_Flag = false; Ticker Tick_Controller; void Set_controller_Flag(){ Controller_Flag = true; } //-------------------------------PI --------------------------------- double PI(double e, const double Kp, const double Ki, double Sf, double &e_int){ //This function is a PID controller that returns the errorsignal for the plant e_int = e_int + Sf * e; // PID return Kp*e + Ki*e_int ; } //---------------------------Controller--------------------------------------- // Controller gains (motor1−Kp,−Ki,...) const double Kpm= 0.1, Kim = 0.005; double e_L_int = 0; double Controller_L(int direction, double signal, double reference ){ //This funcition returns a value that will be sent to drive the motor // - Motor_Direction_Input is the Digital in of the motor // 1 for dirrection means that a clockwise rotation wil be regarded as positive // and -1 for direction means the opposite //PID controller double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_L_int ); //Determining rotational direction of the motor if ((reference-signal*direction >= 0)){ M_L_D = 1; } else { M_L_D = 0; } if (fabs(Speed_Set)< 1){ if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance return 0; } else { return fabs(Speed_Set); } } else { return 1; } } double e_R_int = 0; double Controller_R(int direction, double signal, double reference ){ //This funcition returns a value that will be sent to drive the motor // - Motor_Direction_Input is the Digital in of the motor // 1 for dirrection means that a clockwise rotation wil be regarded as positive // and -1 for direction means the opposite //PI controller double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_R_int); //Determining rotational direction of the motor if ((reference-signal*direction >= 0)){ M_R_D = 0; } else { M_R_D = 1; } if (fabs(Speed_Set)< 1){ if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance return 0; } else { return fabs(Speed_Set); } } else { return 1; } } //-----------------------------Change_Movement---------------------------------- // This function changes the movement type of the robot as a consequence of the input by EMG // Or when the EMG signal is hold for 1.5S The grapler wil activate Ticker Tick_Movement; int check = 0; bool Movement_Flag = false; void Set_movement_Flag(){ Movement_Flag = true; } void Change_Movement(){ if (Movement_Flag == true){ if (EMG_Change > 0.5f ){ check += 1; if (check <= 6 and check > 1 ){ grip1 = 1; } if (check > 6){ grip2 =1; } } else { //Hold the signal shorter than 1.5 seconds if (check <= 6 and check > 1 ){ movement = !movement; grip1 = 1; } // Hold the signal longer than 1.5 seconds if (check > 6){ grip2 =1; Grap = !Grap; } grip2 = 0; grip1 =0; check = 0; } Movement_Flag = false; } } //-----------------------------EMG Signal determinator---------------------- BiQuadChain bqc_L; BiQuadChain bqc_R; BiQuadChain bqc_Change; BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 ); BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 ); BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 ); BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 ); BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 ); BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 ); BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 ); BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 ); BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 ); //HIDScope scope(5); // Sending two sets of data HIDScope scope(3); void getEMG(){ EMG_Change = bqc_Change.step(Change.read()); EMG_Change = fabs(EMG_Change); EMG_Change = bq3_Change.step(EMG_Change)*30; scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope //scope.send(); // scope.set(4,Change.read()); if (EMG_Change < 0.4){ EMG_Change=0; } else { EMG_Change=1.0; } EMG_L = bqc_L.step(Left.read()); EMG_L= fabs(EMG_L); EMG_L= bq3_L.step( EMG_L)*30; scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope if (EMG_L < 0.5){ EMG_L=0; } else { //EMG_L=1.0; } EMG_R = bqc_R.step(Right.read()); EMG_R= fabs(EMG_R); EMG_R= bq3_R.step( EMG_R)*30; scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope if (EMG_R < 0.5){ EMG_R=0; } else { //EMG_R= 1.0; } scope.send(); } //------------------------------------------------------Postition reset---------------------------------- //After the robot has been set in it's equlibrium state, this data is used to prevent the robot of destroying itself. void Position_reset(){ Position_R =0; Position_L =0; } //-------------------------------------------------------Limit------------------------------------- //calculates the angele of the robot with for its limit, and checks of its limit has been reached int config_count = 0; double max_angle = 0; double min_angle = 0.5; int Limit(){ double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm int limit = 0; if (angle < min_angle){ //limit angle of -13 degrees translated to the motor -13*2*pi*136/(33*360) limit = 1; } if (angle > max_angle){ //limit angle of 15 degrees tranlated to the motor 2*15*2*pi*136/(33*360) limit = 2; } return limit; } bool Configure_Flag = true; /////////////-----------------------Booting---------------------------------------= void Boot(){ //Fucnction that initializes variables that have to be initalized within the main of the script //And does the same thing for functions like encoder reset pc.baud(115200); pc.printf("\r\n**BOARD RESET**\r\n"); M_L_S.period(1.0/Motor_Frequency); M_L_S = 0.0; M_R_S.period(1.0/Motor_Frequency); M_R_S= 0.0; rotation = 0; translation = 1; Encoder_L.reset(); Encoder_R.reset(); Grap = 0; grip1 = 0; bqc_L.add( &bq1_L ).add( &bq2_L ); bqc_R.add(&bq1_R).add(&bq2_R); bqc_Change.add(&bq1_Change).add(&bq2_Change); Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency); Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency); Tick_Movement.attach(Set_movement_Flag, 0.25); } int main() { //---------------------Setting constants and system booting--------------------- Boot(); //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/ //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/ //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/ while (true) { if (Sample_Flag == true){ Sample(); getEMG(); // EMG_Change = getEMG( bqc.step(Change.read())); Signal = pi*(EMG_R-EMG_L); //Signal = pi*(Right.read()-Left.read()); //EMG_Change = Change.read(); // scope.set(3,Signal);//------------------------------------------------------------scope // scope.send(); Sample_Flag = false; } //-------------------------------------------- Configureren------------------------------------- if (Configure_Flag == true){ if (config_count < 20){ Signal = pi; } else { Signal = -pi; } if (config_count == 19){ Position_reset(); } if (config_count > 40){ Configure_Flag = false; max_angle = Position_R-Position_L-0.5; } M_L_S = 0.02*Controller_L(direction_L,Signal,Speed_L); M_R_S = 0.02*Controller_R(direction_R,Signal,Speed_R); //Signal = -pi; // M_L_S = 0.03*Controller_L(direction_L,Signal,Speed_L); // M_R_S = 0.03*Controller_R(direction_R,Signal,Speed_R); Grap = 0; if (Movement_Flag == true){ rotation = !rotation; translation =!translation; grip1 = !grip1; grip2 = !grip2; config_count ++; Movement_Flag= false; } } else { //Main statement that states if the system is active or not if (Active.read() > 0.5){ green = 0; red = 1; Change_Movement(); if (Controller_Flag == true){ switch (movement) { case 0: //Clockwise rotation of the robot direction_L = 1; direction_R = 1; rotation = 1; translation = 0; Signal = Signal*2; M_L_S = Controller_L(direction_L,Signal,Speed_L); M_R_S = Controller_R(direction_R,Signal,Speed_R); break; case 1: //Radial movement outwards for a positive value direction_L = 1; direction_R = -1; rotation = 0; translation = 1; switch (Limit()){ //making sure the limits are not passed case 0: //case the robot is in its working space M_L_S = Controller_L(direction_L,Signal,Speed_L); M_R_S = Controller_R(direction_R,Signal,Speed_R); break; case 1: //case the robot has reached it's lowest limit if (Signal < 0){ M_L_S = Controller_L(direction_L,Signal,Speed_L); M_R_S = Controller_R(direction_R,Signal,Speed_R); } else { M_L_S = 0; M_R_S = 0; } break; case 2: //case the robot has reachted its highest limit if (Signal > 0){ M_L_S = Controller_L(direction_L,Signal,Speed_L); M_R_S = Controller_R(direction_R,Signal,Speed_R); } else { M_L_S = 0; M_R_S = 0; } break; } break; } Controller_Flag = false; } } else { if (Active.read() < 0.5){ green = 1; red = 0; M_R_S = 0; M_L_S = 0; Grap=0; } } } } }