| User | Revision | Line number | New contents of line |
|---|
| EvaKrolis |
0:aa126aa2ff8b
|
1
|
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
|
| EvaKrolis |
0:aa126aa2ff8b
|
2
|
// Libraries
|
| EvaKrolis |
0:aa126aa2ff8b
|
3
|
#include "mbed.h"
|
| EvaKrolis |
0:aa126aa2ff8b
|
4
|
#include "BiQuad.h"
|
| EvaKrolis |
0:aa126aa2ff8b
|
5
|
#include "FastPWM.h"
|
| EvaKrolis |
0:aa126aa2ff8b
|
6
|
#include "MODSERIAL.h"
|
| EvaKrolis |
0:aa126aa2ff8b
|
7
|
#include "QEI.h"
|
| EvaKrolis |
0:aa126aa2ff8b
|
8
|
#include <algorithm>
|
| EvaKrolis |
0:aa126aa2ff8b
|
9
|
#include <math.h>
|
| EvaKrolis |
0:aa126aa2ff8b
|
10
|
#include <cmath>
|
| EvaKrolis |
0:aa126aa2ff8b
|
11
|
#define PI 3.14159265
|
| EvaKrolis |
0:aa126aa2ff8b
|
12
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
13
|
// LEDs
|
| EvaKrolis |
0:aa126aa2ff8b
|
14
|
DigitalOut ledRed(LED_RED,1); // red LED K64F
|
| EvaKrolis |
0:aa126aa2ff8b
|
15
|
DigitalOut ledGreen(LED_GREEN,1); // green LED K64F
|
| EvaKrolis |
0:aa126aa2ff8b
|
16
|
DigitalOut ledBlue(LED_BLUE,1); // blue LED K64F
|
| EvaKrolis |
0:aa126aa2ff8b
|
17
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
18
|
Ticker blinkTimer; // LED ticker
|
| EvaKrolis |
0:aa126aa2ff8b
|
19
|
Timer EMGtransition_timer; // Timer for the transition from EMG calibration to homing
|
| EvaKrolis |
0:aa126aa2ff8b
|
20
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
21
|
// Buttons/inputs
|
| EvaKrolis |
0:aa126aa2ff8b
|
22
|
InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
|
| EvaKrolis |
0:aa126aa2ff8b
|
23
|
InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
|
| EvaKrolis |
0:aa126aa2ff8b
|
24
|
InterruptIn buttonEmergency(SW2); // emergency button on K64F
|
| EvaKrolis |
0:aa126aa2ff8b
|
25
|
InterruptIn buttonHome(SW3); // button on K64F
|
| EvaKrolis |
0:aa126aa2ff8b
|
26
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
27
|
// Potmeters
|
| EvaKrolis |
0:aa126aa2ff8b
|
28
|
AnalogIn pot2(A2);
|
| EvaKrolis |
0:aa126aa2ff8b
|
29
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
30
|
// Motor pins input
|
| EvaKrolis |
0:aa126aa2ff8b
|
31
|
DigitalIn pin8(D8); // Encoder L B
|
| EvaKrolis |
0:aa126aa2ff8b
|
32
|
DigitalIn pin9(D9); // Encoder L A
|
| EvaKrolis |
0:aa126aa2ff8b
|
33
|
DigitalIn pin10(D10); // Encoder R B
|
| EvaKrolis |
0:aa126aa2ff8b
|
34
|
DigitalIn pin11(D11); // Encoder R A
|
| EvaKrolis |
0:aa126aa2ff8b
|
35
|
DigitalIn pin12(D12); // Encoder F B
|
| EvaKrolis |
0:aa126aa2ff8b
|
36
|
DigitalIn pin13(D13); // Encoder F A
|
| EvaKrolis |
0:aa126aa2ff8b
|
37
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
38
|
// Motor pins output
|
| EvaKrolis |
0:aa126aa2ff8b
|
39
|
DigitalOut pin2(D2); // Motor F direction = motor flip
|
| EvaKrolis |
0:aa126aa2ff8b
|
40
|
FastPWM pin3(A5); // Motor F pwm = motor flip
|
| EvaKrolis |
0:aa126aa2ff8b
|
41
|
DigitalOut pin4(D4); // Motor R direction = motor right
|
| EvaKrolis |
0:aa126aa2ff8b
|
42
|
FastPWM pin5(D5); // Motor R pwm = motor right
|
| EvaKrolis |
0:aa126aa2ff8b
|
43
|
FastPWM pin6(D6); // Motor L pwm = motor left
|
| EvaKrolis |
0:aa126aa2ff8b
|
44
|
DigitalOut pin7(D7); // Motor L direction = motor left
|
| EvaKrolis |
0:aa126aa2ff8b
|
45
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
46
|
// Utilisation of libraries
|
| EvaKrolis |
0:aa126aa2ff8b
|
47
|
MODSERIAL pc(USBTX, USBRX); // communication with pc
|
| EvaKrolis |
0:aa126aa2ff8b
|
48
|
QEI encoderL(D9,D8,NC,4200); // Encoder input of motor L
|
| EvaKrolis |
0:aa126aa2ff8b
|
49
|
QEI encoderR(D10,D11,NC,4200); // Encoder input of motor R
|
| EvaKrolis |
0:aa126aa2ff8b
|
50
|
QEI encoderF(D12,D13,NC,4200); // Encoder input of motor F
|
| EvaKrolis |
0:aa126aa2ff8b
|
51
|
Ticker motorL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
52
|
Ticker motorR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
53
|
Ticker motorF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
54
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
55
|
// Define & initialise state machine
|
| EvaKrolis |
0:aa126aa2ff8b
|
56
|
const float dt = 0.002f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
57
|
enum states { calibratingMotorL, calibratingMotorR, calibratingMotorF,
|
| EvaKrolis |
0:aa126aa2ff8b
|
58
|
calibratingEMG, homing, reading, operating, failing, demoing
|
| EvaKrolis |
0:aa126aa2ff8b
|
59
|
};
|
| EvaKrolis |
0:aa126aa2ff8b
|
60
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
61
|
states currentState = calibratingMotorL; // start in motor L mode
|
| EvaKrolis |
0:aa126aa2ff8b
|
62
|
bool changeState = true; // initialise the first state
|
| EvaKrolis |
0:aa126aa2ff8b
|
63
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
64
|
Ticker stateTimer; // state machine ticker
|
| EvaKrolis |
0:aa126aa2ff8b
|
65
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
66
|
//------------------------ Parameters for the EMG ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
67
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
68
|
// EMG inputs
|
| EvaKrolis |
0:aa126aa2ff8b
|
69
|
AnalogIn EMG0In(A0); // EMG input 0
|
| EvaKrolis |
0:aa126aa2ff8b
|
70
|
AnalogIn EMG1In(A1); // EMG input 1
|
| EvaKrolis |
0:aa126aa2ff8b
|
71
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
72
|
// Timers
|
| EvaKrolis |
0:aa126aa2ff8b
|
73
|
Ticker FindMax0_timer; // Timer for finding the max muscle
|
| EvaKrolis |
0:aa126aa2ff8b
|
74
|
Ticker FindMax1_timer; // Timer for finding the max muscle
|
| EvaKrolis |
0:aa126aa2ff8b
|
75
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
76
|
// Constants
|
| EvaKrolis |
0:aa126aa2ff8b
|
77
|
const int Length = 10000; // Length of the array for the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
78
|
const int Parts = 50; // Mean average filter over 50 values
|
| EvaKrolis |
0:aa126aa2ff8b
|
79
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
80
|
// Parameters for the first EMG signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
81
|
float EMG0; // float for EMG input
|
| EvaKrolis |
0:aa126aa2ff8b
|
82
|
float EMG0filt; // float for filtered EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
83
|
float EMG0filtArray[Parts]; // Array for the filtered array
|
| EvaKrolis |
0:aa126aa2ff8b
|
84
|
float EMG0Average; // float for the value after Moving Average Filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
85
|
float Sum0 = 0; // Sum0 for the moving average filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
86
|
float EMG0Calibrate[Length]; // Array for the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
87
|
int ReadCal0 = 0; // Integer to read over the calibration array
|
| EvaKrolis |
0:aa126aa2ff8b
|
88
|
float MaxValue0 = 0; // float to save the max muscle
|
| EvaKrolis |
0:aa126aa2ff8b
|
89
|
float Threshold0 = 0; // Threshold for the first EMG signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
90
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
91
|
// Parameters for the second EMG signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
92
|
float EMG1; // float for EMG input
|
| EvaKrolis |
0:aa126aa2ff8b
|
93
|
float EMG1filt; // float for filtered EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
94
|
float EMG1filtArray[Parts]; // Array for the filtered array
|
| EvaKrolis |
0:aa126aa2ff8b
|
95
|
float EMG1Average; // float for the value after Moving Average Filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
96
|
float Sum1 = 0; // Sum0 for the moving average filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
97
|
float EMG1Calibrate[Length]; // Array for the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
98
|
int ReadCal1 = 0; // Integer to read over the calibration array
|
| EvaKrolis |
0:aa126aa2ff8b
|
99
|
float MaxValue1 = 0; // float to save the max muscle
|
| EvaKrolis |
0:aa126aa2ff8b
|
100
|
float Threshold1 = 0; // Threshold for the second EMG signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
101
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
102
|
//Filter variables
|
| EvaKrolis |
0:aa126aa2ff8b
|
103
|
BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
104
|
BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
105
|
BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
106
|
BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
107
|
BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
108
|
BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
109
|
BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
110
|
BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
111
|
BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
112
|
BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
113
|
BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
114
|
BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
115
|
BiQuadChain filter0; //Make chain of filters for the first EMG signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
116
|
BiQuadChain filter1; //Make chain of filters for the second EMG signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
117
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
118
|
//Bool for movement
|
| EvaKrolis |
0:aa126aa2ff8b
|
119
|
bool xMove = false; //Bool for the x-movement
|
| EvaKrolis |
0:aa126aa2ff8b
|
120
|
bool yMove = false; //Bool for the y-movement
|
| EvaKrolis |
0:aa126aa2ff8b
|
121
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
122
|
// -------------------- Parameters for the kinematics -------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
123
|
double theta1 = PI*0.40; //Angle of the left motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
124
|
double theta4 = PI*0.40; //Angle of the right motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
125
|
// ---------------------- Parameters for the motors ---------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
126
|
const float countsRad = 4200.0f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
127
|
int countsL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
128
|
int countsR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
129
|
int countsF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
130
|
int countsCalibratedL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
131
|
int countsCalibratedR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
132
|
int countsCalibratedF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
133
|
int countsOffsetL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
134
|
int countsOffsetR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
135
|
int countsOffsetF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
136
|
float angleCurrentL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
137
|
float angleCurrentR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
138
|
float angleCurrentF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
139
|
float errorL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
140
|
float errorR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
141
|
float errorF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
142
|
float errorCalibratedL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
143
|
float errorCalibratedR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
144
|
float errorCalibratedF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
145
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
146
|
int countsCalibration = 4200/4;
|
| EvaKrolis |
0:aa126aa2ff8b
|
147
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
148
|
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
|
| EvaKrolis |
0:aa126aa2ff8b
|
149
|
// ============================ GENERAL FUNCTIONS =============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
150
|
void stopProgram(void)
|
| EvaKrolis |
0:aa126aa2ff8b
|
151
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
152
|
// Error message
|
| EvaKrolis |
0:aa126aa2ff8b
|
153
|
pc.printf("[ERROR] emergency button pressed\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
154
|
currentState = failing; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
155
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
156
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
157
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
158
|
void blinkLedRed(void)
|
| EvaKrolis |
0:aa126aa2ff8b
|
159
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
160
|
ledRed =! ledRed; // toggle LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
161
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
162
|
void blinkLedGreen(void)
|
| EvaKrolis |
0:aa126aa2ff8b
|
163
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
164
|
ledGreen =! ledGreen; // toggle LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
165
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
166
|
void blinkLedBlue(void)
|
| EvaKrolis |
0:aa126aa2ff8b
|
167
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
168
|
ledBlue =! ledBlue; // toggle LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
169
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
170
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
171
|
float angleCurrent(float counts) // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
|
| EvaKrolis |
0:aa126aa2ff8b
|
172
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
173
|
float angle = ((float)counts*2.0f*PI)/countsRad;
|
| EvaKrolis |
0:aa126aa2ff8b
|
174
|
while (angle > 2.0f*PI) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
175
|
angle = angle-2.0f*PI;
|
| EvaKrolis |
0:aa126aa2ff8b
|
176
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
177
|
while (angle < -2.0f*PI) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
178
|
angle = angle+2.0f*PI;
|
| EvaKrolis |
0:aa126aa2ff8b
|
179
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
180
|
return angle;
|
| EvaKrolis |
0:aa126aa2ff8b
|
181
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
182
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
183
|
// ============================= EMG FUNCTIONS ===============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
184
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
185
|
//Function to read and filter the EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
186
|
void ReadUseEMG0()
|
| EvaKrolis |
0:aa126aa2ff8b
|
187
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
188
|
for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
|
| EvaKrolis |
0:aa126aa2ff8b
|
189
|
EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
|
| EvaKrolis |
0:aa126aa2ff8b
|
190
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
191
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
192
|
Sum0 = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
193
|
EMG0 = EMG0In; //Save EMG input in float
|
| EvaKrolis |
0:aa126aa2ff8b
|
194
|
EMG0filt = filter0.step(EMG0); //Filter the signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
195
|
EMG0filt = abs(EMG0filt); //Take the absolute value
|
| EvaKrolis |
0:aa126aa2ff8b
|
196
|
EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
|
| EvaKrolis |
0:aa126aa2ff8b
|
197
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
198
|
for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
199
|
Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
|
| EvaKrolis |
0:aa126aa2ff8b
|
200
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
201
|
EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
|
| EvaKrolis |
0:aa126aa2ff8b
|
202
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
203
|
if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
|
| EvaKrolis |
0:aa126aa2ff8b
|
204
|
xMove = true; //Set movement to true
|
| EvaKrolis |
0:aa126aa2ff8b
|
205
|
} else {
|
| EvaKrolis |
0:aa126aa2ff8b
|
206
|
xMove = false; //Otherwise set movement to false
|
| EvaKrolis |
0:aa126aa2ff8b
|
207
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
208
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
209
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
210
|
//Function to read and filter the EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
211
|
void ReadUseEMG1()
|
| EvaKrolis |
0:aa126aa2ff8b
|
212
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
213
|
for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
|
| EvaKrolis |
0:aa126aa2ff8b
|
214
|
EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
|
| EvaKrolis |
0:aa126aa2ff8b
|
215
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
216
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
217
|
Sum1 = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
218
|
EMG1 = EMG1In; //Save EMG input in float
|
| EvaKrolis |
0:aa126aa2ff8b
|
219
|
EMG1filt = filter1.step(EMG1); //Filter the signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
220
|
EMG1filt = abs(EMG1filt); //Take the absolute value
|
| EvaKrolis |
0:aa126aa2ff8b
|
221
|
EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
|
| EvaKrolis |
0:aa126aa2ff8b
|
222
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
223
|
for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
224
|
Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
|
| EvaKrolis |
0:aa126aa2ff8b
|
225
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
226
|
EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
|
| EvaKrolis |
0:aa126aa2ff8b
|
227
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
228
|
if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
|
| EvaKrolis |
0:aa126aa2ff8b
|
229
|
yMove = true; //Set y movement to true
|
| EvaKrolis |
0:aa126aa2ff8b
|
230
|
} else {
|
| EvaKrolis |
0:aa126aa2ff8b
|
231
|
yMove = false; //Otherwise set y movement to false
|
| EvaKrolis |
0:aa126aa2ff8b
|
232
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
233
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
234
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
235
|
//Function to make an array during the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
236
|
void CalibrateEMG0()
|
| EvaKrolis |
0:aa126aa2ff8b
|
237
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
238
|
for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
|
| EvaKrolis |
0:aa126aa2ff8b
|
239
|
EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
|
| EvaKrolis |
0:aa126aa2ff8b
|
240
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
241
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
242
|
Sum0 = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
243
|
EMG0 = EMG0In; //Save EMG input in float
|
| EvaKrolis |
0:aa126aa2ff8b
|
244
|
EMG0filt = filter0.step(EMG0); //Filter the signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
245
|
EMG0filt = abs(EMG0filt); //Take the absolute value
|
| EvaKrolis |
0:aa126aa2ff8b
|
246
|
EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
|
| EvaKrolis |
0:aa126aa2ff8b
|
247
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
248
|
for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
249
|
Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
|
| EvaKrolis |
0:aa126aa2ff8b
|
250
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
251
|
EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
|
| EvaKrolis |
0:aa126aa2ff8b
|
252
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
253
|
ReadCal0++;
|
| EvaKrolis |
0:aa126aa2ff8b
|
254
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
255
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
256
|
//Function to make an array during the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
257
|
void CalibrateEMG1()
|
| EvaKrolis |
0:aa126aa2ff8b
|
258
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
259
|
for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
|
| EvaKrolis |
0:aa126aa2ff8b
|
260
|
EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
|
| EvaKrolis |
0:aa126aa2ff8b
|
261
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
262
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
263
|
Sum1 = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
264
|
EMG1 = EMG1In; //Save EMG input in float
|
| EvaKrolis |
0:aa126aa2ff8b
|
265
|
EMG1filt = filter1.step(EMG1); //Filter the signal
|
| EvaKrolis |
0:aa126aa2ff8b
|
266
|
EMG1filt = abs(EMG1filt); //Take the absolute value
|
| EvaKrolis |
0:aa126aa2ff8b
|
267
|
EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
|
| EvaKrolis |
0:aa126aa2ff8b
|
268
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
269
|
for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
|
| EvaKrolis |
0:aa126aa2ff8b
|
270
|
Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
|
| EvaKrolis |
0:aa126aa2ff8b
|
271
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
272
|
EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
|
| EvaKrolis |
0:aa126aa2ff8b
|
273
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
274
|
ReadCal1++;
|
| EvaKrolis |
0:aa126aa2ff8b
|
275
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
276
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
277
|
//Function to find the max value during the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
278
|
void FindMax0()
|
| EvaKrolis |
0:aa126aa2ff8b
|
279
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
280
|
MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
|
| EvaKrolis |
0:aa126aa2ff8b
|
281
|
Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
|
| EvaKrolis |
0:aa126aa2ff8b
|
282
|
pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
|
| EvaKrolis |
0:aa126aa2ff8b
|
283
|
FindMax0_timer.detach(); //Detach the timer, so you only use this once
|
| EvaKrolis |
0:aa126aa2ff8b
|
284
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
285
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
286
|
//Function to find the max value during the calibration
|
| EvaKrolis |
0:aa126aa2ff8b
|
287
|
void FindMax1()
|
| EvaKrolis |
0:aa126aa2ff8b
|
288
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
289
|
MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
|
| EvaKrolis |
0:aa126aa2ff8b
|
290
|
Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
|
| EvaKrolis |
0:aa126aa2ff8b
|
291
|
pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
|
| EvaKrolis |
0:aa126aa2ff8b
|
292
|
FindMax1_timer.detach(); //Detach the timer, so you only use this once
|
| EvaKrolis |
0:aa126aa2ff8b
|
293
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
294
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
295
|
// ================ FUNCTIONS TO LINK THE EMG TO THE MOTORS===================
|
| EvaKrolis |
0:aa126aa2ff8b
|
296
|
void EMGMotorLink(){
|
| EvaKrolis |
0:aa126aa2ff8b
|
297
|
if (xMove == true){
|
| EvaKrolis |
0:aa126aa2ff8b
|
298
|
theta1 = theta1*0.0001;
|
| EvaKrolis |
0:aa126aa2ff8b
|
299
|
theta4 = -theta4*0.0001;
|
| EvaKrolis |
0:aa126aa2ff8b
|
300
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
301
|
if(yMove == true){
|
| EvaKrolis |
0:aa126aa2ff8b
|
302
|
theta1 = -theta1*0.0001;
|
| EvaKrolis |
0:aa126aa2ff8b
|
303
|
theta4 = theta4*0.0001;
|
| EvaKrolis |
0:aa126aa2ff8b
|
304
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
305
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
306
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
307
|
// ============================ MOTOR FUNCTIONS ===============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
308
|
// angleDesired now defined as angle of potmeter and not the angle as output from the kinematics
|
| EvaKrolis |
0:aa126aa2ff8b
|
309
|
// So, the angleDesired needs to be defined again and the part in which the potmeter value is calculated needs to be commented
|
| EvaKrolis |
0:aa126aa2ff8b
|
310
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
311
|
// ------------------------ General motor functions ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
312
|
int countsInputL() // Gets the counts from encoder 1
|
| EvaKrolis |
0:aa126aa2ff8b
|
313
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
314
|
countsL = encoderL.getPulses();
|
| EvaKrolis |
0:aa126aa2ff8b
|
315
|
return countsL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
316
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
317
|
int countsInputR() // Gets the counts from encoder 2
|
| EvaKrolis |
0:aa126aa2ff8b
|
318
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
319
|
countsR = encoderR.getPulses();
|
| EvaKrolis |
0:aa126aa2ff8b
|
320
|
return countsR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
321
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
322
|
int countsInputF() // Gets the counts from encoder 3
|
| EvaKrolis |
0:aa126aa2ff8b
|
323
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
324
|
countsF = encoderF.getPulses();
|
| EvaKrolis |
0:aa126aa2ff8b
|
325
|
return countsF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
326
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
327
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
328
|
float errorCalc(float angleReference,float angleCurrent) // Calculates the error of the system, based on the current angle and the reference value
|
| EvaKrolis |
0:aa126aa2ff8b
|
329
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
330
|
float error = angleReference - angleCurrent;
|
| EvaKrolis |
0:aa126aa2ff8b
|
331
|
return error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
332
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
333
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
334
|
float angleDesired() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
|
| EvaKrolis |
0:aa126aa2ff8b
|
335
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
336
|
float angle = (pot2*2.0f*PI)-PI;
|
| EvaKrolis |
0:aa126aa2ff8b
|
337
|
return angle;
|
| EvaKrolis |
0:aa126aa2ff8b
|
338
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
339
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
340
|
// ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
341
|
float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
342
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
343
|
float Kp = 19.24f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
344
|
float Ki = 1.02f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
345
|
float Kd = 0.827f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
346
|
float error = errorCalc(angleReference,angleCurrent);
|
| EvaKrolis |
0:aa126aa2ff8b
|
347
|
static float errorIntegralL = 0.0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
348
|
static float errorPreviousL = error; // initialization with this value only done once!
|
| EvaKrolis |
0:aa126aa2ff8b
|
349
|
static BiQuad PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
|
| EvaKrolis |
0:aa126aa2ff8b
|
350
|
// Proportional part:
|
| EvaKrolis |
0:aa126aa2ff8b
|
351
|
float u_k = Kp * error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
352
|
// Integral part
|
| EvaKrolis |
0:aa126aa2ff8b
|
353
|
errorIntegralL = errorIntegralL + error * dt;
|
| EvaKrolis |
0:aa126aa2ff8b
|
354
|
float u_i = Ki * errorIntegralL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
355
|
// Derivative part
|
| EvaKrolis |
0:aa126aa2ff8b
|
356
|
float errorDerivative = (error - errorPreviousL)/dt;
|
| EvaKrolis |
0:aa126aa2ff8b
|
357
|
float errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
|
| EvaKrolis |
0:aa126aa2ff8b
|
358
|
float u_d = Kd * errorDerivativeFiltered;
|
| EvaKrolis |
0:aa126aa2ff8b
|
359
|
errorPreviousL = error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
360
|
// Sum all parts and return it
|
| EvaKrolis |
0:aa126aa2ff8b
|
361
|
return u_k + u_i + u_d;
|
| EvaKrolis |
0:aa126aa2ff8b
|
362
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
363
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
364
|
int countsInputCalibratedL() // Gets the counts from encoder 1
|
| EvaKrolis |
0:aa126aa2ff8b
|
365
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
366
|
countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
|
| EvaKrolis |
0:aa126aa2ff8b
|
367
|
return countsL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
368
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
369
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
370
|
void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
|
| EvaKrolis |
0:aa126aa2ff8b
|
371
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
372
|
float angleReferenceL = theta1; // insert kinematics output here instead of angleDesired()
|
| EvaKrolis |
0:aa126aa2ff8b
|
373
|
angleReferenceL = -angleReferenceL; // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
374
|
countsL = countsInputCalibratedL(); // different encoder pins per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
375
|
angleCurrentL = angleCurrent(countsL); // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
376
|
errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
377
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
378
|
float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
379
|
pin6 = fabs(PIDControlL); // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
380
|
pin7 = PIDControlL > 0.0f; // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
381
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
382
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
383
|
void calibrationL() // Partially the same as motorTurnL, only with potmeter input
|
| EvaKrolis |
0:aa126aa2ff8b
|
384
|
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
|
| EvaKrolis |
0:aa126aa2ff8b
|
385
|
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
|
| EvaKrolis |
0:aa126aa2ff8b
|
386
|
// Do this for every motor and after calibrated all motors, press a button
|
| EvaKrolis |
0:aa126aa2ff8b
|
387
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
388
|
float angleReferenceL = angleDesired(); // insert kinematics output here instead of angleDesired()
|
| EvaKrolis |
0:aa126aa2ff8b
|
389
|
angleReferenceL = -angleReferenceL; // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
390
|
countsL = countsInputL(); // different encoder pins per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
391
|
angleCurrentL = angleCurrent(countsL); // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
392
|
errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
393
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
394
|
if (fabs(errorL) >= 0.01f) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
395
|
float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
396
|
pin6 = fabs(PIDControlL); // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
397
|
pin7 = PIDControlL > 0.0f; // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
398
|
} else if (fabs(errorL) < 0.01f) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
399
|
countsOffsetL = countsL;
|
| EvaKrolis |
0:aa126aa2ff8b
|
400
|
countsL = countsL - countsOffsetL + countsCalibration;
|
| EvaKrolis |
0:aa126aa2ff8b
|
401
|
//countsL = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
402
|
pin6 = 0.0f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
403
|
// BUTTON PRESS: TO NEXT STATE
|
| EvaKrolis |
0:aa126aa2ff8b
|
404
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
405
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
406
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
407
|
// ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
408
|
float PIDControllerR(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
409
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
410
|
float Kp = 19.24f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
411
|
float Ki = 1.02f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
412
|
float Kd = 0.827f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
413
|
float error = errorCalc(angleReference,angleCurrent);
|
| EvaKrolis |
0:aa126aa2ff8b
|
414
|
static float errorIntegralR = 0.0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
415
|
static float errorPreviousR = error; // initialization with this value only done once!
|
| EvaKrolis |
0:aa126aa2ff8b
|
416
|
static BiQuad PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
|
| EvaKrolis |
0:aa126aa2ff8b
|
417
|
// Proportional part:
|
| EvaKrolis |
0:aa126aa2ff8b
|
418
|
float u_k = Kp * error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
419
|
// Integral part
|
| EvaKrolis |
0:aa126aa2ff8b
|
420
|
errorIntegralR = errorIntegralR + error * dt;
|
| EvaKrolis |
0:aa126aa2ff8b
|
421
|
float u_i = Ki * errorIntegralR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
422
|
// Derivative part
|
| EvaKrolis |
0:aa126aa2ff8b
|
423
|
float errorDerivative = (error - errorPreviousR)/dt;
|
| EvaKrolis |
0:aa126aa2ff8b
|
424
|
float errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
|
| EvaKrolis |
0:aa126aa2ff8b
|
425
|
float u_d = Kd * errorDerivativeFiltered;
|
| EvaKrolis |
0:aa126aa2ff8b
|
426
|
errorPreviousR = error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
427
|
// Sum all parts and return it
|
| EvaKrolis |
0:aa126aa2ff8b
|
428
|
return u_k + u_i + u_d;
|
| EvaKrolis |
0:aa126aa2ff8b
|
429
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
430
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
431
|
int countsInputCalibratedR() // Gets the counts from encoder 1
|
| EvaKrolis |
0:aa126aa2ff8b
|
432
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
433
|
countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
|
| EvaKrolis |
0:aa126aa2ff8b
|
434
|
return countsR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
435
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
436
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
437
|
void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
|
| EvaKrolis |
0:aa126aa2ff8b
|
438
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
439
|
float angleReferenceR = theta4; // insert kinematics output here instead of angleDesired()
|
| EvaKrolis |
0:aa126aa2ff8b
|
440
|
angleReferenceR = -angleReferenceR; // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
441
|
countsR = countsInputCalibratedR(); // different encoder pins per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
442
|
angleCurrentR = angleCurrent(countsR); // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
443
|
errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
444
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
445
|
float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
446
|
pin5 = fabs(PIDControlR); // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
447
|
pin4 = PIDControlR > 0.0f; // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
448
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
449
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
450
|
void calibrationR() // Partially the same as motorTurnR, only with potmeter input
|
| EvaKrolis |
0:aa126aa2ff8b
|
451
|
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
|
| EvaKrolis |
0:aa126aa2ff8b
|
452
|
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
|
| EvaKrolis |
0:aa126aa2ff8b
|
453
|
// Do this for every motor and after calibrated all motors, press a button
|
| EvaKrolis |
0:aa126aa2ff8b
|
454
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
455
|
float angleReferenceR = angleDesired(); // insert kinematics output here instead of angleDesired()
|
| EvaKrolis |
0:aa126aa2ff8b
|
456
|
angleReferenceR = -angleReferenceR; // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
457
|
countsR = countsInputR(); // different encoder pins per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
458
|
angleCurrentR = angleCurrent(countsR); // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
459
|
errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
460
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
461
|
if (fabs(errorR) >= 0.01f) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
462
|
float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
463
|
pin5 = fabs(PIDControlR); // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
464
|
pin4 = PIDControlR > 0.0f; // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
465
|
} else if (fabs(errorR) < 0.01f) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
466
|
countsOffsetR = countsR;
|
| EvaKrolis |
0:aa126aa2ff8b
|
467
|
countsR = countsR - countsOffsetR + countsCalibration;
|
| EvaKrolis |
0:aa126aa2ff8b
|
468
|
//countsR = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
469
|
pin5 = 0.0f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
470
|
// BUTTON PRESS: TO NEXT STATE
|
| EvaKrolis |
0:aa126aa2ff8b
|
471
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
472
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
473
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
474
|
// ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
475
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
476
|
float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
477
|
{ float Kp = 19.24f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
478
|
float Ki = 1.02f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
479
|
float Kd = 0.827f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
480
|
float error = errorCalc(angleReference,angleCurrent);
|
| EvaKrolis |
0:aa126aa2ff8b
|
481
|
static float errorIntegralF = 0.0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
482
|
static float errorPreviousF = error; // initialization with this value only done once!
|
| EvaKrolis |
0:aa126aa2ff8b
|
483
|
static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
|
| EvaKrolis |
0:aa126aa2ff8b
|
484
|
// Proportional part:
|
| EvaKrolis |
0:aa126aa2ff8b
|
485
|
float u_k = Kp * error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
486
|
// Integral part
|
| EvaKrolis |
0:aa126aa2ff8b
|
487
|
errorIntegralF = errorIntegralF + error * dt;
|
| EvaKrolis |
0:aa126aa2ff8b
|
488
|
float u_i = Ki * errorIntegralF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
489
|
// Derivative part
|
| EvaKrolis |
0:aa126aa2ff8b
|
490
|
float errorDerivative = (error - errorPreviousF)/dt;
|
| EvaKrolis |
0:aa126aa2ff8b
|
491
|
float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
|
| EvaKrolis |
0:aa126aa2ff8b
|
492
|
float u_d = Kd * errorDerivativeFiltered;
|
| EvaKrolis |
0:aa126aa2ff8b
|
493
|
errorPreviousF = error;
|
| EvaKrolis |
0:aa126aa2ff8b
|
494
|
// Sum all parts and return it
|
| EvaKrolis |
0:aa126aa2ff8b
|
495
|
return u_k + u_i + u_d;
|
| EvaKrolis |
0:aa126aa2ff8b
|
496
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
497
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
498
|
int countsInputCalibratedF() // Gets the counts from encoder 1
|
| EvaKrolis |
0:aa126aa2ff8b
|
499
|
{ countsF = encoderF.getPulses()- countsOffsetF + countsCalibration;
|
| EvaKrolis |
0:aa126aa2ff8b
|
500
|
return countsF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
501
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
502
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
503
|
void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
|
| EvaKrolis |
0:aa126aa2ff8b
|
504
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
505
|
float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
|
| EvaKrolis |
0:aa126aa2ff8b
|
506
|
angleReferenceF = -angleReferenceF; // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
507
|
countsF = countsInputCalibratedF(); // different encoder pins per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
508
|
angleCurrentF = angleCurrent(countsF); // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
509
|
errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
510
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
511
|
float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
512
|
pin5 = fabs(PIDControlF); // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
513
|
pin4 = PIDControlF > 0.0f; // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
514
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
515
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
516
|
void calibrationF() // Partially the same as motorTurnF, only with potmeter input
|
| EvaKrolis |
0:aa126aa2ff8b
|
517
|
// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
|
| EvaKrolis |
0:aa126aa2ff8b
|
518
|
// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
|
| EvaKrolis |
0:aa126aa2ff8b
|
519
|
// Do this for every motor and after calibrated all motors, press a button
|
| EvaKrolis |
0:aa126aa2ff8b
|
520
|
{ float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
|
| EvaKrolis |
0:aa126aa2ff8b
|
521
|
angleReferenceF = -angleReferenceF; // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
522
|
countsF = countsInputF(); // different encoder pins per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
523
|
angleCurrentF = angleCurrent(countsF); // different minus sign per motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
524
|
errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
525
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
526
|
if (fabs(errorF) >= 0.01f)
|
| EvaKrolis |
0:aa126aa2ff8b
|
527
|
{ float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
528
|
pin5 = fabs(PIDControlF); // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
529
|
pin4 = PIDControlF > 0.0f; // different pins for every motor
|
| EvaKrolis |
0:aa126aa2ff8b
|
530
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
531
|
else if (fabs(errorF) < 0.01f)
|
| EvaKrolis |
0:aa126aa2ff8b
|
532
|
{ countsOffsetF = countsF;
|
| EvaKrolis |
0:aa126aa2ff8b
|
533
|
countsF = countsF - countsOffsetF + countsCalibration;
|
| EvaKrolis |
0:aa126aa2ff8b
|
534
|
//countsF = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
535
|
pin5 = 0.0f;
|
| EvaKrolis |
0:aa126aa2ff8b
|
536
|
// BUTTON PRESS: TO NEXT STATE
|
| EvaKrolis |
0:aa126aa2ff8b
|
537
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
538
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
539
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
540
|
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
|
| EvaKrolis |
0:aa126aa2ff8b
|
541
|
void stateMachine(void)
|
| EvaKrolis |
0:aa126aa2ff8b
|
542
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
543
|
switch (currentState) { // determine which state Odin is in
|
| EvaKrolis |
0:aa126aa2ff8b
|
544
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
545
|
// ======================== MOTOR L CALIBRATION MODE =========================
|
| EvaKrolis |
0:aa126aa2ff8b
|
546
|
case calibratingMotorL:
|
| EvaKrolis |
0:aa126aa2ff8b
|
547
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
548
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
549
|
pc.printf("[MODE] calibrating motor L...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
550
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
551
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
552
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
553
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
554
|
ledRed = 1; // green blinking LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
555
|
ledGreen = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
556
|
ledBlue = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
557
|
blinkTimer.attach(&blinkLedGreen,0.5);
|
| EvaKrolis |
0:aa126aa2ff8b
|
558
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
559
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
560
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
561
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
562
|
calibrationL();
|
| EvaKrolis |
0:aa126aa2ff8b
|
563
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
564
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
565
|
// Transition condition: when all motor errors smaller than 0.01,
|
| EvaKrolis |
0:aa126aa2ff8b
|
566
|
// start calibrating EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
567
|
if (errorL < 0.01f && buttonBio1 == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
568
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
569
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
570
|
blinkTimer.detach();
|
| EvaKrolis |
0:aa126aa2ff8b
|
571
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
572
|
currentState = calibratingMotorR; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
573
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
574
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
575
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
576
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
577
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
578
|
// ======================== MOTOR R CALIBRATION MODE =========================
|
| EvaKrolis |
0:aa126aa2ff8b
|
579
|
case calibratingMotorR:
|
| EvaKrolis |
0:aa126aa2ff8b
|
580
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
581
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
582
|
pc.printf("[MODE] calibrating motor R...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
583
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
584
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
585
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
586
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
587
|
ledRed = 1; // cyan-green blinking LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
588
|
ledGreen = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
589
|
ledBlue = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
590
|
blinkTimer.attach(&blinkLedBlue,0.5);
|
| EvaKrolis |
0:aa126aa2ff8b
|
591
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
592
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
593
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
594
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
595
|
calibrationR();
|
| EvaKrolis |
0:aa126aa2ff8b
|
596
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
597
|
// Transition condition: when all motor errors smaller than 0.01,
|
| EvaKrolis |
0:aa126aa2ff8b
|
598
|
// start calibrating EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
599
|
if (errorR < 0.01f && buttonBio2 == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
600
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
601
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
602
|
blinkTimer.detach();
|
| EvaKrolis |
0:aa126aa2ff8b
|
603
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
604
|
currentState = calibratingMotorF; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
605
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
606
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
607
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
608
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
609
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
610
|
// ======================== MOTOR F CALIBRATION MODE =========================
|
| EvaKrolis |
0:aa126aa2ff8b
|
611
|
case calibratingMotorF:
|
| EvaKrolis |
0:aa126aa2ff8b
|
612
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
613
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
614
|
pc.printf("[MODE] calibrating motor F...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
615
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
616
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
617
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
618
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
619
|
ledRed = 1; // green blinking LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
620
|
ledGreen = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
621
|
ledBlue = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
622
|
blinkTimer.attach(&blinkLedGreen,0.5);
|
| EvaKrolis |
0:aa126aa2ff8b
|
623
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
624
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
625
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
626
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
627
|
calibrationF();
|
| EvaKrolis |
0:aa126aa2ff8b
|
628
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
629
|
// Transition condition: when all motor errors smaller than 0.01,
|
| EvaKrolis |
0:aa126aa2ff8b
|
630
|
// start calibrating EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
631
|
if (errorF < 0.01f && buttonHome == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
632
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
633
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
634
|
blinkTimer.detach();
|
| EvaKrolis |
0:aa126aa2ff8b
|
635
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
636
|
currentState = calibratingEMG; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
637
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
638
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
639
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
640
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
641
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
642
|
// =========================== EMG CALIBRATION MODE ===========================
|
| EvaKrolis |
0:aa126aa2ff8b
|
643
|
case calibratingEMG:
|
| EvaKrolis |
0:aa126aa2ff8b
|
644
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
645
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
646
|
pc.printf("[MODE] calibrating EMG...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
647
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
648
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
649
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
650
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
651
|
ledRed = 1; // cyan-blue blinking LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
652
|
ledGreen = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
653
|
ledBlue = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
654
|
blinkTimer.attach(&blinkLedGreen,0.5);
|
| EvaKrolis |
0:aa126aa2ff8b
|
655
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
656
|
FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
|
| EvaKrolis |
0:aa126aa2ff8b
|
657
|
FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
|
| EvaKrolis |
0:aa126aa2ff8b
|
658
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
659
|
EMGtransition_timer.reset();
|
| EvaKrolis |
0:aa126aa2ff8b
|
660
|
EMGtransition_timer.start();
|
| EvaKrolis |
0:aa126aa2ff8b
|
661
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
662
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
663
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
664
|
CalibrateEMG0(); //start emg calibration every 0.005 seconds
|
| EvaKrolis |
0:aa126aa2ff8b
|
665
|
CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
|
| EvaKrolis |
0:aa126aa2ff8b
|
666
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
667
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
668
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
669
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
670
|
// Transition condition: after 20 sec in state
|
| EvaKrolis |
0:aa126aa2ff8b
|
671
|
if (EMGtransition_timer.read() > 20) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
672
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
673
|
blinkTimer.detach();
|
| EvaKrolis |
0:aa126aa2ff8b
|
674
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
675
|
currentState = homing; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
676
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
677
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
678
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
679
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
680
|
// ============================== HOMING MODE ================================
|
| EvaKrolis |
0:aa126aa2ff8b
|
681
|
case homing:
|
| EvaKrolis |
0:aa126aa2ff8b
|
682
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
683
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
684
|
pc.printf("[MODE] homing...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
685
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
686
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
687
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
688
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
689
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
690
|
ledRed = 1; // cyan LED on
|
| EvaKrolis |
0:aa126aa2ff8b
|
691
|
ledGreen = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
692
|
ledBlue = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
693
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
694
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
695
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
696
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
697
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
698
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
699
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
700
|
// Transition condition #1: with button press, enter reading mode,
|
| EvaKrolis |
0:aa126aa2ff8b
|
701
|
// but only when velocity == 0
|
| EvaKrolis |
0:aa126aa2ff8b
|
702
|
if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
|
| EvaKrolis |
0:aa126aa2ff8b
|
703
|
&& errorCalibratedF < 0.01f && buttonBio1 == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
704
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
705
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
706
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
707
|
currentState = reading; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
708
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
709
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
710
|
// Transition condition #2: with button press, enter demo mode
|
| EvaKrolis |
0:aa126aa2ff8b
|
711
|
// but only when velocity == 0
|
| EvaKrolis |
0:aa126aa2ff8b
|
712
|
if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
|
| EvaKrolis |
0:aa126aa2ff8b
|
713
|
&& errorCalibratedF < 0.01f && buttonBio2 == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
714
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
715
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
716
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
717
|
currentState = demoing; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
718
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
719
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
720
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
721
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
722
|
// ============================== READING MODE ===============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
723
|
case reading:
|
| EvaKrolis |
0:aa126aa2ff8b
|
724
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
725
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
726
|
pc.printf("[MODE] reading...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
727
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
728
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
729
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
730
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
731
|
ledRed = 1; // blue LED on
|
| EvaKrolis |
0:aa126aa2ff8b
|
732
|
ledGreen = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
733
|
ledBlue = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
734
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
735
|
// TERUGKLAPPEN
|
| EvaKrolis |
0:aa126aa2ff8b
|
736
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
737
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
738
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
739
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
740
|
ReadUseEMG0(); //Start the use of EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
741
|
ReadUseEMG1(); //Start the use of EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
742
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
743
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
744
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
745
|
// Transition condition #1: when EMG signal detected, enter operating
|
| EvaKrolis |
0:aa126aa2ff8b
|
746
|
// mode
|
| EvaKrolis |
0:aa126aa2ff8b
|
747
|
if (xMove == true && yMove == true) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
748
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
749
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
750
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
751
|
currentState = operating; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
752
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
753
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
754
|
// Transition condition #2: with button press, back to homing mode
|
| EvaKrolis |
0:aa126aa2ff8b
|
755
|
if (buttonHome == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
756
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
757
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
758
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
759
|
currentState = homing; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
760
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
761
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
762
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
763
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
764
|
// ============================= OPERATING MODE ==============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
765
|
case operating:
|
| EvaKrolis |
0:aa126aa2ff8b
|
766
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
767
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
768
|
pc.printf("[MODE] operating...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
769
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
770
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
771
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
772
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
773
|
ledRed = 1; // blue fast blinking LED
|
| EvaKrolis |
0:aa126aa2ff8b
|
774
|
ledGreen = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
775
|
ledBlue = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
776
|
blinkTimer.attach(&blinkLedBlue,0.25);
|
| EvaKrolis |
0:aa126aa2ff8b
|
777
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
778
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
779
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
780
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
781
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
782
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
783
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
784
|
// Transition condition: when path is over, back to reading mode
|
| EvaKrolis |
0:aa126aa2ff8b
|
785
|
if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
786
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
787
|
blinkTimer.detach();
|
| EvaKrolis |
0:aa126aa2ff8b
|
788
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
789
|
currentState = reading; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
790
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
791
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
792
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
793
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
794
|
// ============================== DEMOING MODE ===============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
795
|
case demoing:
|
| EvaKrolis |
0:aa126aa2ff8b
|
796
|
// ------------------------- initialisation --------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
797
|
if (changeState) { // when entering the state
|
| EvaKrolis |
0:aa126aa2ff8b
|
798
|
pc.printf("[MODE] demoing...\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
799
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
800
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
801
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
802
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
803
|
ledRed = 0; // yellow LED on
|
| EvaKrolis |
0:aa126aa2ff8b
|
804
|
ledGreen = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
805
|
ledBlue = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
806
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
807
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
808
|
// ----------------------------- action ------------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
809
|
// Actions for each loop iteration
|
| EvaKrolis |
0:aa126aa2ff8b
|
810
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
811
|
ReadUseEMG0(); //Start the use of EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
812
|
ReadUseEMG1(); //Start the use of EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
813
|
pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
|
| EvaKrolis |
0:aa126aa2ff8b
|
814
|
motorTurnL();
|
| EvaKrolis |
0:aa126aa2ff8b
|
815
|
motorTurnR();
|
| EvaKrolis |
0:aa126aa2ff8b
|
816
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
817
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
818
|
// --------------------------- transition ----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
819
|
// Transition condition: with button press, back to homing mode
|
| EvaKrolis |
0:aa126aa2ff8b
|
820
|
if (buttonHome == false) {
|
| EvaKrolis |
0:aa126aa2ff8b
|
821
|
// Actions when leaving state
|
| EvaKrolis |
0:aa126aa2ff8b
|
822
|
/* */
|
| EvaKrolis |
0:aa126aa2ff8b
|
823
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
824
|
currentState = homing; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
825
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
826
|
}
|
| EvaKrolis |
0:aa126aa2ff8b
|
827
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
828
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
829
|
// =============================== FAILING MODE ================================
|
| EvaKrolis |
0:aa126aa2ff8b
|
830
|
case failing:
|
| EvaKrolis |
0:aa126aa2ff8b
|
831
|
changeState = false; // stay in this state
|
| EvaKrolis |
0:aa126aa2ff8b
|
832
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
833
|
// Actions when entering state
|
| EvaKrolis |
0:aa126aa2ff8b
|
834
|
ledRed = 0; // red LED on
|
| EvaKrolis |
0:aa126aa2ff8b
|
835
|
ledGreen = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
836
|
ledBlue = 1;
|
| EvaKrolis |
0:aa126aa2ff8b
|
837
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
838
|
pin3 = 0; // all motor forces to zero
|
| EvaKrolis |
0:aa126aa2ff8b
|
839
|
pin5 = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
840
|
pin6 = 0;
|
| EvaKrolis |
0:aa126aa2ff8b
|
841
|
exit (0); // stop all current functions
|
| EvaKrolis |
0:aa126aa2ff8b
|
842
|
break; // end case
|
| EvaKrolis |
0:aa126aa2ff8b
|
843
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
844
|
// ============================== DEFAULT MODE =================================
|
| EvaKrolis |
0:aa126aa2ff8b
|
845
|
default:
|
| EvaKrolis |
0:aa126aa2ff8b
|
846
|
// ---------------------------- enter failing mode -----------------------------
|
| EvaKrolis |
0:aa126aa2ff8b
|
847
|
currentState = failing; // change to state
|
| EvaKrolis |
0:aa126aa2ff8b
|
848
|
changeState = true; // next loop, switch states
|
| EvaKrolis |
0:aa126aa2ff8b
|
849
|
// print current state
|
| EvaKrolis |
0:aa126aa2ff8b
|
850
|
pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
851
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
852
|
} // end switch
|
| EvaKrolis |
0:aa126aa2ff8b
|
853
|
} // end stateMachine
|
| EvaKrolis |
0:aa126aa2ff8b
|
854
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
855
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
856
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
857
|
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
|
| EvaKrolis |
0:aa126aa2ff8b
|
858
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
859
|
int main()
|
| EvaKrolis |
0:aa126aa2ff8b
|
860
|
{
|
| EvaKrolis |
0:aa126aa2ff8b
|
861
|
// ================================ EMERGENCY ================================
|
| EvaKrolis |
0:aa126aa2ff8b
|
862
|
//If the emergency button is pressed, stop program via failing state
|
| EvaKrolis |
0:aa126aa2ff8b
|
863
|
buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
|
| EvaKrolis |
0:aa126aa2ff8b
|
864
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
865
|
// ============================= PC-COMMUNICATION ============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
866
|
pc.baud(115200); // communication with terminal
|
| EvaKrolis |
0:aa126aa2ff8b
|
867
|
pc.printf("\n\n[START] starting O.D.I.N\r\n");
|
| EvaKrolis |
0:aa126aa2ff8b
|
868
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
869
|
// ============================= PIN DEFINE PERIOD ===========================
|
| EvaKrolis |
0:aa126aa2ff8b
|
870
|
// If you give a period on one pin, c++ gives all pins this period
|
| EvaKrolis |
0:aa126aa2ff8b
|
871
|
pin3.period_us(15);
|
| EvaKrolis |
0:aa126aa2ff8b
|
872
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
873
|
// ==================================== LOOP ==================================
|
| EvaKrolis |
0:aa126aa2ff8b
|
874
|
// run state machine at 500 Hz
|
| EvaKrolis |
0:aa126aa2ff8b
|
875
|
stateTimer.attach(&stateMachine,dt);
|
| EvaKrolis |
0:aa126aa2ff8b
|
876
|
|
| EvaKrolis |
0:aa126aa2ff8b
|
877
|
// =============================== ADD FILTERS ===============================
|
| EvaKrolis |
0:aa126aa2ff8b
|
878
|
//Make filter chain for the first EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
879
|
filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
|
| EvaKrolis |
0:aa126aa2ff8b
|
880
|
//Make filter chain for the second EMG
|
| EvaKrolis |
0:aa126aa2ff8b
|
881
|
filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
|
| EvaKrolis |
0:aa126aa2ff8b
|
882
|
}
|
Eva Krolis
