Fabian van Hummel / Mbed 2 deprecated Robot_control_v3

biorobotics group 19 , 2 november

Dependencies:   HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed

Fork of Robot_controlv2 by Fabian van Hummel

Controller.h@4:bfdcf3da7cff, 2016-10-28 (annotated)

Committer:
fabian101
Date:
Fri Oct 28 12:49:02 2016 +0000
Revision:
4:bfdcf3da7cff
Parent:
2:6b913f93bc0b
Child:
6:a4440eec3652
28-10, filtering needs tweaking, controller needs testing, some fix-me's need to be done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabian101 4:bfdcf3da7cff 1 #include "SigInterpreter.h"
fabian101 4:bfdcf3da7cff 2
fabian101 4:bfdcf3da7cff 3 SigInterpreter signal = SigInterpreter();
fabian101 1:b9005f2aabf5 4 const double PI = 3.141592653589793;
fabian101 4:bfdcf3da7cff 5 //MODSERIAL pc(USBTX, USBRX);
fabian101 1:b9005f2aabf5 6
fabian101 2:6b913f93bc0b 7 int xPulses = 0; // pulsecount
fabian101 4:bfdcf3da7cff 8 const int x_max = 33068;
fabian101 2:6b913f93bc0b 9 int yPulses = 0;
fabian101 4:bfdcf3da7cff 10 const int y_max = 17640;
fabian101 2:6b913f93bc0b 11 int prevXpulses = 0;
fabian101 2:6b913f93bc0b 12 int prevYpulses = 0;
fabian101 1:b9005f2aabf5 13
fabian101 4:bfdcf3da7cff 14 double rMax = 0; // maxAmplitude emg signal
fabian101 4:bfdcf3da7cff 15 double lMax = 0;
fabian101 4:bfdcf3da7cff 16 double uMax = 0;
fabian101 4:bfdcf3da7cff 17 double dMax = 0;
fabian101 1:b9005f2aabf5 18
fabian101 2:6b913f93bc0b 19 float motorValueX = 0; // requested speed
fabian101 2:6b913f93bc0b 20 float motorValueY = 0;
fabian101 0:d935ea6f5c25 21
fabian101 2:6b913f93bc0b 22 const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s
fabian101 2:6b913f93bc0b 23 const float maxYValue = 1.0f; //
fabian101 1:b9005f2aabf5 24
fabian101 2:6b913f93bc0b 25 const float maxSpeed = 0.01f; // m/s
fabian101 0:d935ea6f5c25 26
fabian101 2:6b913f93bc0b 27 const float R1 = 0.085; // radius of big driven gear in m
fabian101 2:6b913f93bc0b 28 const float RmX = 0.012; // radius of small drive gear in m
fabian101 2:6b913f93bc0b 29 const float Rpulley = 0.015915; // radius of pulley gear in m
fabian101 1:b9005f2aabf5 30
fabian101 2:6b913f93bc0b 31 float prevX = 0;
fabian101 1:b9005f2aabf5 32 float prevY = 0;
fabian101 1:b9005f2aabf5 33
fabian101 2:6b913f93bc0b 34 double r = 0;
fabian101 2:6b913f93bc0b 35 double l = 0;
fabian101 2:6b913f93bc0b 36 double u = 0;
fabian101 2:6b913f93bc0b 37 double d = 0;
fabian101 4:bfdcf3da7cff 38 int i = 0;
fabian101 2:6b913f93bc0b 39
fabian101 4:bfdcf3da7cff 40 const int SAMPLES_PER_AVERAGE = 10;
fabian101 1:b9005f2aabf5 41 const int margin = 100; // margin pulses for calibration
fabian101 0:d935ea6f5c25 42
fabian101 0:d935ea6f5c25 43 void calibrate();
fabian101 1:b9005f2aabf5 44 void calibratePower();
fabian101 0:d935ea6f5c25 45 void run();
fabian101 0:d935ea6f5c25 46 double toMotorValue(double,double);
fabian101 0:d935ea6f5c25 47 void emergencyExit();
fabian101 0:d935ea6f5c25 48 void motorOutput(double, bool);
fabian101 1:b9005f2aabf5 49 void initControllers();
fabian101 1:b9005f2aabf5 50 void initMotors();
fabian101 2:6b913f93bc0b 51 void initMotorvariables();
fabian101 0:d935ea6f5c25 52
fabian101 4:bfdcf3da7cff 53 HIDScope scope(2);
fabian101 0:d935ea6f5c25 54
fabian101 1:b9005f2aabf5 55 const float Kc = 0.0;
fabian101 1:b9005f2aabf5 56 const float Ti = 0.0;
fabian101 1:b9005f2aabf5 57 const float Td = 0.0;
fabian101 4:bfdcf3da7cff 58 const float RATE = 0.001; // rate = interval motors will be updateed
fabian101 4:bfdcf3da7cff 59
fabian101 4:bfdcf3da7cff 60 int counter = 0;
fabian101 4:bfdcf3da7cff 61 int counter_motor = 0;
fabian101 2:6b913f93bc0b 62
fabian101 2:6b913f93bc0b 63 PwmOut X_Magnitude(D5); // bind to horizontal motor
fabian101 2:6b913f93bc0b 64 DigitalOut X_Direction(D4);
fabian101 2:6b913f93bc0b 65 PID X_Controller(Kc, Ti, Td, RATE);
fabian101 4:bfdcf3da7cff 66 QEI X_Motor(D12 ,D13 ,NC, 64 ,QEI::X4_ENCODING);
fabian101 1:b9005f2aabf5 67
fabian101 2:6b913f93bc0b 68 PwmOut Y_Magnitude(D6); // bind to Ytical motor
fabian101 2:6b913f93bc0b 69 DigitalOut Y_Direction(D7);
fabian101 2:6b913f93bc0b 70 PID Y_Controller(Kc, Ti, Td, RATE);
fabian101 4:bfdcf3da7cff 71 QEI Y_Motor(D14 ,D15 ,NC, 64 ,QEI::X4_ENCODING);
fabian101 1:b9005f2aabf5 72
fabian101 4:bfdcf3da7cff 73 //InterruptIn calibrateButton(D2);
fabian101 4:bfdcf3da7cff 74 DigitalIn button(D2);
fabian101 0:d935ea6f5c25 75 Ticker motorControl;
fabian101 4:bfdcf3da7cff 76 void setUpdate_flag();
fabian101 4:bfdcf3da7cff 77 volatile bool update_flag = false;
fabian101 4:bfdcf3da7cff 78 const int CW = true;
fabian101 2:6b913f93bc0b 79
fabian101 4:bfdcf3da7cff 80 DigitalOut led(LED_BLUE);
fabian101 1:b9005f2aabf5 81
fabian101 1:b9005f2aabf5 82 void initMotors(){
fabian101 4:bfdcf3da7cff 83 X_Magnitude.period_ms(1);
fabian101 2:6b913f93bc0b 84 X_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 85 X_Direction.write(CW);
fabian101 1:b9005f2aabf5 86
fabian101 4:bfdcf3da7cff 87 Y_Magnitude.period_ms(1);
fabian101 2:6b913f93bc0b 88 Y_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 89 Y_Direction.write(CW);
fabian101 1:b9005f2aabf5 90 }
fabian101 1:b9005f2aabf5 91 void initControllers(){
fabian101 4:bfdcf3da7cff 92 X_Controller.setInputLimits(-1.0,1.0); // fiX_ me
fabian101 2:6b913f93bc0b 93 X_Controller.setOutputLimits(-1.0,1.0);
fabian101 2:6b913f93bc0b 94 X_Controller.setBias(0);
fabian101 4:bfdcf3da7cff 95 X_Controller.setMode(0); // fix me
fabian101 1:b9005f2aabf5 96
fabian101 4:bfdcf3da7cff 97 Y_Controller.setInputLimits(-1.0,1.0); // fix me
fabian101 2:6b913f93bc0b 98 Y_Controller.setOutputLimits(-1.0,1.0);
fabian101 2:6b913f93bc0b 99 Y_Controller.setBias(0);
fabian101 2:6b913f93bc0b 100 Y_Controller.setMode(0);
fabian101 1:b9005f2aabf5 101 }