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@7:2ab4bdd1f998, 2016-11-02 (annotated)

Committer:
fabian101
Date:
Wed Nov 02 15:40:15 2016 +0000
Revision:
7:2ab4bdd1f998
Parent:
6:a4440eec3652 
biorobotics group 19 , 2 november

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 6:a4440eec3652 5
fabian101 6:a4440eec3652 6 MODSERIAL pc(USBTX, USBRX);
fabian101 6:a4440eec3652 7 //HIDScope scope(2);
fabian101 1:b9005f2aabf5 8
fabian101 2:6b913f93bc0b 9 int xPulses = 0; // pulsecount
fabian101 6:a4440eec3652 10 const int x_max = 30068;// 30068;
fabian101 2:6b913f93bc0b 11 int yPulses = 0;
fabian101 6:a4440eec3652 12 const int y_max = 15003;
fabian101 2:6b913f93bc0b 13 int prevXpulses = 0;
fabian101 2:6b913f93bc0b 14 int prevYpulses = 0;
fabian101 1:b9005f2aabf5 15
fabian101 2:6b913f93bc0b 16 float motorValueX = 0; // requested speed
fabian101 2:6b913f93bc0b 17 float motorValueY = 0;
fabian101 0:d935ea6f5c25 18
fabian101 6:a4440eec3652 19 const float maxXValue = 1.0; // max motorvalue
fabian101 6:a4440eec3652 20 const float maxYValue = 1.0;
fabian101 1:b9005f2aabf5 21
fabian101 6:a4440eec3652 22 const float maxSpeed = 1; // cm/s
fabian101 6:a4440eec3652 23
fabian101 6:a4440eec3652 24 float X_frictionTrheshold = 0.04;
fabian101 6:a4440eec3652 25 float Y_frictionTrheshold = 0.04;
fabian101 6:a4440eec3652 26 float sensitivityFactor = 0.2; // determines treshold value for contraction, value between 0 and 1
fabian101 0:d935ea6f5c25 27
fabian101 2:6b913f93bc0b 28 const float R1 = 0.085; // radius of big driven gear in m
fabian101 2:6b913f93bc0b 29 const float RmX = 0.012; // radius of small drive gear in m
fabian101 2:6b913f93bc0b 30 const float Rpulley = 0.015915; // radius of pulley gear in m
fabian101 1:b9005f2aabf5 31
fabian101 2:6b913f93bc0b 32 float prevX = 0;
fabian101 1:b9005f2aabf5 33 float prevY = 0;
fabian101 1:b9005f2aabf5 34
fabian101 6:a4440eec3652 35 float r = 0;
fabian101 6:a4440eec3652 36 float l = 0;
fabian101 6:a4440eec3652 37 float u = 0;
fabian101 6:a4440eec3652 38 float d = 0;
fabian101 6:a4440eec3652 39 float rMax = 0; // maxAmplitude emg signal
fabian101 6:a4440eec3652 40 float lMax = 0;
fabian101 6:a4440eec3652 41 float uMax = 0;
fabian101 6:a4440eec3652 42 float dMax = 0;
fabian101 6:a4440eec3652 43 float percentageR = 0;
fabian101 6:a4440eec3652 44 float percentageL = 0;
fabian101 6:a4440eec3652 45 float percentageU = 0;
fabian101 6:a4440eec3652 46 float percentageD = 0;
fabian101 6:a4440eec3652 47
fabian101 4:bfdcf3da7cff 48 int i = 0;
fabian101 2:6b913f93bc0b 49
fabian101 1:b9005f2aabf5 50 const int margin = 100; // margin pulses for calibration
fabian101 0:d935ea6f5c25 51
fabian101 0:d935ea6f5c25 52 void calibrate();
fabian101 1:b9005f2aabf5 53 void calibratePower();
fabian101 0:d935ea6f5c25 54 void run();
fabian101 1:b9005f2aabf5 55 void initControllers();
fabian101 1:b9005f2aabf5 56 void initMotors();
fabian101 0:d935ea6f5c25 57
fabian101 6:a4440eec3652 58 const float KcX = 0.0358; // THIS IS THE LIMIT FOR STABILITY
fabian101 6:a4440eec3652 59 const float KcY = 0.366;
fabian101 1:b9005f2aabf5 60 const float Ti = 0.0;
fabian101 1:b9005f2aabf5 61 const float Td = 0.0;
fabian101 6:a4440eec3652 62 const float RATE = 0.002; // rate = interval motors will be updateed
fabian101 4:bfdcf3da7cff 63
fabian101 6:a4440eec3652 64 int counter = 1;
fabian101 2:6b913f93bc0b 65
fabian101 2:6b913f93bc0b 66 PwmOut X_Magnitude(D5); // bind to horizontal motor
fabian101 2:6b913f93bc0b 67 DigitalOut X_Direction(D4);
fabian101 6:a4440eec3652 68 PID X_Controller(KcX, Ti, Td, RATE);
fabian101 6:a4440eec3652 69 QEI X_Motor(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING);
fabian101 1:b9005f2aabf5 70
fabian101 6:a4440eec3652 71 PwmOut Y_Magnitude(D6); // bind to vertical motor
fabian101 2:6b913f93bc0b 72 DigitalOut Y_Direction(D7);
fabian101 6:a4440eec3652 73 PID Y_Controller(KcY, Ti, Td, RATE);
fabian101 6:a4440eec3652 74 QEI Y_Motor(D15 ,D14 ,NC, 64 ,QEI::X4_ENCODING);
fabian101 1:b9005f2aabf5 75
fabian101 4:bfdcf3da7cff 76 //InterruptIn calibrateButton(D2);
fabian101 4:bfdcf3da7cff 77 DigitalIn button(D2);
fabian101 0:d935ea6f5c25 78 Ticker motorControl;
fabian101 4:bfdcf3da7cff 79 void setUpdate_flag();
fabian101 4:bfdcf3da7cff 80 volatile bool update_flag = false;
fabian101 6:a4440eec3652 81 const int CW = false;
fabian101 2:6b913f93bc0b 82
fabian101 4:bfdcf3da7cff 83 DigitalOut led(LED_BLUE);
fabian101 1:b9005f2aabf5 84
fabian101 1:b9005f2aabf5 85 void initMotors(){
fabian101 4:bfdcf3da7cff 86 X_Magnitude.period_ms(1);
fabian101 2:6b913f93bc0b 87 X_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 88 X_Direction.write(CW);
fabian101 1:b9005f2aabf5 89
fabian101 4:bfdcf3da7cff 90 Y_Magnitude.period_ms(1);
fabian101 2:6b913f93bc0b 91 Y_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 92 Y_Direction.write(CW);
fabian101 6:a4440eec3652 93 led = true;
fabian101 1:b9005f2aabf5 94 }
fabian101 1:b9005f2aabf5 95 void initControllers(){
fabian101 6:a4440eec3652 96 X_Controller.setInputLimits(-maxSpeed,maxSpeed); // max output will be 10%
fabian101 2:6b913f93bc0b 97 X_Controller.setOutputLimits(-1.0,1.0);
fabian101 2:6b913f93bc0b 98 X_Controller.setBias(0);
fabian101 6:a4440eec3652 99 X_Controller.setMode(1);
fabian101 1:b9005f2aabf5 100
fabian101 6:a4440eec3652 101 Y_Controller.setInputLimits(-maxSpeed,maxSpeed); // fix me
fabian101 2:6b913f93bc0b 102 Y_Controller.setOutputLimits(-1.0,1.0);
fabian101 2:6b913f93bc0b 103 Y_Controller.setBias(0);
fabian101 6:a4440eec3652 104 Y_Controller.setMode(1);
fabian101 1:b9005f2aabf5 105 }