Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics
Fork of
Minor_test_serial
by 
First Last
Revision 39:d065ad7a978d, committed 2017-10-26
- Comitter:
- tvlogman
- Date:
- Thu Oct 26 13:29:56 2017 +0000
- Parent:
- 38:f1d2d42a4bdc
- Child:
- 40:7418f46a1ac0
- Commit message:
- Working version implementing Tom Lankhorst's BiQuad library and using position control.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Thu Oct 26 13:29:56 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath.lib Thu Oct 26 13:29:56 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Yo_Robot/code/MatrixMath/#93948a9bbde2
--- a/biquadChain.lib Tue Oct 24 13:17:42 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/tvlogman/code/biquadChain/#8b742e1512c1
--- a/biquadFilter.lib Tue Oct 24 13:17:42 2017 +0000 +++ b/biquadFilter.lib Thu Oct 26 13:29:56 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/tvlogman/code/biquadFilter/#8589bd80071d +http://mbed.org/users/tomlankhorst/code/biquadFilter/#26861979d305
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inverseKinematics.lib Thu Oct 26 13:29:56 2017 +0000 @@ -0,0 +1,1 @@ +inverseKinematics#537f81d7b756
--- a/main.cpp Tue Oct 24 13:17:42 2017 +0000
+++ b/main.cpp Thu Oct 26 13:29:56 2017 +0000
@@ -1,6 +1,7 @@
#include <vector>
#include <numeric>
#include "mbed.h"
+#include "Matrix.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
@@ -9,8 +10,7 @@
#include "controller.h"
#include "motorConfig.h"
#include "errorFetch.h"
-#include "biquadFilter.h"
-#include "biquadChain.h"
+#include "BiQuad.h"
// ADJUSTABLE PARAMETERS
// controller ticker time interval
@@ -24,13 +24,15 @@
// high pass
-biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
-biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
-biquadChain HPbqc(HPbq1, HPbq2);
+BiQuadChain HPbqc;
+BiQuad HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
+BiQuad HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
+
+
// low pass
-biquadFilter LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924);
-biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
-biquadChain LPbqc(LPbq1, LPbq2);
+BiQuadChain LPbqc;
+BiQuad LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924);
+BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
// Controller parameters
const float k_p = 1;
@@ -83,19 +85,23 @@
volatile float z;
volatile float q;
volatile float k;
+volatile float w;
-void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
+
+void sendDataToPc(float data1, float data2, float data3, float data4, float data5, float data6){
// Capture data
x = data1;
y = data2;
z = data3;
q = data4;
k = data5;
+ w = data6;
scope.set(0, x);
scope.set(1, y);
scope.set(2, z);
scope.set(3, q);
scope.set(4, z);
+ scope.set(5, w);
scope.send(); // send what's in scope memory to PC
}
@@ -129,8 +135,14 @@
float r2_uf = ref2.getReference();
pc.printf("In controller ticker \r\n");
- float r1 = r1_uf;
- float r2 = r2_uf;
+ // Filter reference
+ float r1 = HPbqc.step(r1_uf);
+ r1 = fabs(r1);
+ r1 = LPbqc.step(r1);
+
+ float r2 = HPbqc.step(r2_uf);
+ r2 = fabs(r2);
+ r2 = LPbqc.step(r2);
// Finite state machine
switch(currentState){
@@ -155,7 +167,7 @@
e1.fetchError(m1counts, r1);
e2.fetchError(m2counts, r2);
- sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0); // just send the EMG signal value to HIDscope
+ sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope
break;
}
@@ -164,16 +176,6 @@
if(stateChanged){
pc.printf("Active state \r\n");
}
- // Filter reference
- float r1 = HPbqc.applyFilter(r1_uf);
- r1 = fabs(r1);
- r1 = LPbqc.applyFilter(r1);
-
- float r2 = HPbqc.applyFilter(r2_uf);
- r2 = fabs(r2);
- r2 = LPbqc.applyFilter(r2);
-
-
// Compute error
e1.fetchError(m1counts, r1);
@@ -186,7 +188,7 @@
motor2.setMotor(motorValue2);
// Send data to HIDscope
- sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0);
+ sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
// Set LED to blue
ledR = 1;
@@ -241,7 +243,7 @@
ledR = 1;
ledG = 0;
ledB = 1;
- sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0);
+ sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0, 0.0);
break;
}
}
@@ -279,6 +281,9 @@
{
pc.baud(115200);
pc.printf("Main function");
+ HPbqc.add(&HPbq1).add(&HPbq2);
+ LPbqc.add(&LPbq1).add(&LPbq2);
+
// Attaching state change functions to buttons;
sw2.fall(&killSwitch);
sw3.fall(&activateRobot);