Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- Renate
- Date:
- 2019-11-05
- Revision:
- 43:2f946b617d62
- Parent:
- 42:ca4bbc3e0239
File content as of revision 43:2f946b617d62:
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#define M_PI 3.14159265358979323846 /* pi */
#include <math.h>
#include "Servo.h"
#include <cmath>
#include <complex>
Serial pc(USBTX, USBRX);
// TICKERS
Ticker loop_ticker; // Ticker aanmaken die ervoor zorgt dat de ProcessStateMachine met een frequentie vsn 500 Hz kan worden aangeroepen.
// BENODIGD VOOR PROCESS STATE MACHINE
enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode}; // Alle states definiëren.
states currentState = Motors_off; // State waarin wordt begonnen definiëren.
bool stateChanged = true; // Toevoegen zodat de initialisatie van de eerste state plaatsvindt.
// INPUTS
DigitalIn Power_button_pressed(D1); // Definiëren van alle buttons, we gebruiken hiervoor geen InterruptIn, maar DigitalIn.
DigitalIn Emergency_button_pressed(D2);
DigitalIn Motor_calib_button_pressed(SW2);
DigitalIn servo_button_pressed(SW3);
AnalogIn EMG_biceps_right_raw (A0); // Definiëren van de ruwe EMG-signalen die binnenkomen via AnalogIn.
AnalogIn EMG_biceps_left_raw (A1); // We gebruiken signalen van de kuit en de linker en rechter biceps.
AnalogIn EMG_calf_raw (A2);
QEI Encoder1(D13, D12, NC, 64); // Definities voor de encoders op motor 1 (Encoder1) en 2 (Encoder2). Hiervoor wordt de QEI library gebruikt
QEI Encoder2(D10, D9, NC, 64); // We gebruiken X2 encoding, wat standaard is en dus niet hoeft worden toegevoegd aan deze defninitie.
// Het aantal counts per omwenteling is gelijk aan 64.
// OUTPUTS
PwmOut motor1(D6); // Definities voor de motorsnelheden door middel van PwmOut. Er kan een getal tussen 0 en 1 worden ingevoerd.
PwmOut motor2(D5);
DigitalOut motor1_dir(D7); // Definities voor de richtingen van de motoren. Het getal 0 zorgt voor de ene richting, het getal 1 voor de andere.
DigitalOut motor2_dir(D4); // In ons geval zijn beide motoren rechtsom draaiend vanaf de assen bekeken, wanneer de richting op 1 wordt gezet.
Servo mijnServo(D7); // Definitie voor de output naar de servo motor, benodigd voor het controleren van de spatelpositie.
// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
double counts1; // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor
double counts2; // de offset opstellen.
double offset1 = 0.0;
double offset2 = 0.0;
const double conversion_factor = (2.0*M_PI)/((64.0*131.25)/2); // Omrekeningsfactor om de encoder counts om te zetten naar de huidige motorhoek.
double theta_h_1_rad; // Actuele motorhoek in radialen (motor 1).
double theta_h_2_rad; // Actuele motorhoek in radialen (motor 2).
double q1; // Hoek joint 1 die volgt uit de actuele motorhoeken.
double q2; // Hoek joint 2 die volgt uit de actuele motorhoeken.
// DEFINITIES VOOR FILTERS
// BICEPS-RECHTS
// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
BiQuadChain bqcbr;
BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
BiQuadChain bqcbr2;
BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
// BICEPS-LINKS
// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
BiQuadChain bqcbl;
BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
BiQuadChain bqcbl2;
BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
// KUIT
// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
BiQuadChain bqck;
BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
BiQuadChain bqck2;
BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
// VARIABELEN VOOR EMG + FILTEREN
double filtered_EMG_biceps_right_1; // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter.
double filtered_EMG_biceps_left_1;
double filtered_EMG_calf_1;
double filtered_EMG_biceps_right_abs; // Definities voor de signalen, waarbij de absolute waarden genomen zijn van de eerste filterketen.
double filtered_EMG_biceps_left_abs;
double filtered_EMG_calf_abs;
double filtered_EMG_biceps_right; // Definities voor de gefilterde EMG-signalen, na de tweede filter keten.
double filtered_EMG_biceps_left;
double filtered_EMG_calf;
// Variabelen voor HIDScope
HIDScope scope(3); // Op deze manier kunnen drie signalen worden weergegeven in HIDScope.
// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
bool calib = false; // Benodigde bool en 'runner' (i_calib) voor het uitvoeren van de EMG-kalbratie.
static int i_calib = 0;
double filtered_EMG_biceps_right_total; // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie.
double filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden.
double filtered_EMG_calf_total;
double mean_EMG_biceps_right; // Global variables definiëren voor het gemiddelde maximale EMG-signaal, verkregen d.m.v. de EMG-kalibratie.
double mean_EMG_biceps_left;
double mean_EMG_calf;
// VARIABELEN VOOR OPERATION MODE (EMG)
double normalized_EMG_biceps_right; // Global variables definiëren voor het normaliseren van het gemeten EMG-signaal naar het gemiddelde maximale EMG-signaal.
double normalized_EMG_biceps_left;
double normalized_EMG_calf;
// VARIABELEN VOOR OPERATION MODE (RKI)
double vx; // Geeft de definitie voor de 'desired velocity' in x-richting.
double vy; // Geeft de definitie voor de 'desired velocity' in y-richting.
double Inverse_jacobian[2][2]; // Matrixen opstellen voor de inverse jacobian en de gewenste snelheden (vx en vy).
double desired_velocity[2][1];
double Joint_velocity[2][1] = {{0.0}, {0.0}}; // Nulmatrix opstellen voor de joint snelheden, beginwaarden.
const double delta_t = 0.002; // Tijdsverschil dat wordt gebruikt om de joint velocities te integreren, is gelijk aan de duur van een 'ticker ronde'.
double Joint_1_position = 0.0; // Begindefinities opstellen voor de joint en motor posities.
double Joint_2_position = 0.0;
double Joint_1_position_prev = 0.0;
double Joint_2_position_prev = 0.0;
double Motor_1_position = 0.0;
double Motor_2_position = 0.0;
double error_M1; // Definiëren van de motorerrors, het verschil tussen de daadwerkelijke hoek en de gewenste hoek.
double error_M2;
// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
const double Kp = 17.5; // Kp en Ki waarden voor de PI-controller definiëren.
const double Ki = 1.02; // Zijn theoretisch getest met goede resultaten, in de praktijk konden geen goede tests worden verricht, doordat de
// robot oncontroleerbaar gedrag vertoonde.
double theta_k_1 = 0.0; // Begin definities opstellen voor de motorhoeken na de proportional part.
double theta_k_2 = 0.0;
double error_integral_1 = 0.0; // Begin definities opstellen voor de integralen van de errors.
double error_integral_2 = 0.0;
double u_i_1; // Uitkomst variabelen definiëren voor de vermenigvuldiging van de error integraal met Ki.
double u_i_2;
double theta_t_1; // Variabelen opstellen voor de sommatie van theta_k en u_i.
double theta_t_2;
double abs_theta_t_1; // Variabele opstellen voor de absolute waarde van theta_t.
double abs_theta_t_2;
// VARIABELEN VOOR OPERATION MODE (SERVO)
double theta_f= 580; // Dit zijn allemaal waarden tussen de 500 en 2400.
double theta_sout; // Dit zijn allemaal waarden tussen de 500 en 2400.
double q1_ser; // Dit zijn allemaal waarden tussen de 500 en 2400.
double q2_ser; // Dit zijn allemaal waarden tussen de 500 en 2400.
// VOIDS
void emergency() // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld
{ // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna
loop_ticker.detach(); // het script opnieuw moet worden opgestart.
motor1.write(0);
motor2.write(0);
pc.printf("Ik ga exploderen!!!\r\n");
}
void motors_off() // Functie waarbij beide motoren worden uitgezet.
{
motor1.write(0);
motor2.write(0);
pc.printf("Motoren uit functie\r\n");
}
void EMG_calibration() // Functie die wordt uitgevoerd tijdens de EMG kalibratie. Wordt geinitialiseerd door de bool calib,
{ // die alleen waar is tijdens de EMG kalibratie. Er wordt gebruikgemaakt van een runner i_calib, zodat
if (i_calib == 0) { // EMG-waarden tijdens maximale spierspanning gedurende 5 seconden bij elkaar op worden geteld. Het gemiddelde
filtered_EMG_biceps_right_total=0; // hiervan kan worden bepaald door te delen door het aantal samples dat genomen is in dit interval, namelijk 2500.
filtered_EMG_biceps_left_total=0;
filtered_EMG_calf_total=0;
}
if (i_calib <= 2500) {
filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
filtered_EMG_calf_total+=filtered_EMG_calf;
i_calib++;
}
if (i_calib > 2500) {
mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
mean_EMG_calf=filtered_EMG_calf_total/2500.0;
pc.printf("Ontspan spieren\r\n");
pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
calib = false;
}
}
void Homing_function() // Functie die ervoor zorgt dat homing wordt uitgevoerd. De motoren worden met een lage snelheid bewogen, tot de motorhoeken
{ // weer in de referentie positie staan (0 in ons geval, doordat we compenseren met een offset voor encoder counts). Wanneer
if (theta_h_1_rad != 0.0) { // deze positie is bereikt, wordt de snelheid op nul gezet. In de ProcessStateMachine wordt overgegaan naar de volgende state,
if (theta_h_1_rad < 0) { // wanneer dit voor beide motoren is gebeurd.
motor1.write(0.3);
motor1_dir.write(0);
} else {
motor1.write(0.3);
motor1_dir.write(1);
}
}
if (theta_h_1_rad == 0.0) {
motor1.write(0);
}
if (theta_h_2_rad != 0.0) {
if (theta_h_2_rad < 0) {
motor2.write(0.3);
motor2_dir.write(0);
} else {
motor2.write(0.3);
motor2_dir.write(1);
}
}
if (theta_h_2_rad == 0.0) {
motor2.write(0);
}
}
void Inverse_Kinematics() // Functie die de inverse kinematica uitvoert. De inverse Jacobian is afkosmtig uit eerdere berekeningen in Matlab.
{ // The desired velocity komt voort uit de operation mode, waar hier een waarde aan wordt toegekend.
// Defining joint velocities based on calculations of matlab // Outputs van deze functie zijn motorposities, die later worden gebruikt bij het berekenen van een positie error.
Inverse_jacobian[0][0] = ((cos(q1+3.141592653589793/6.0)*-8.5E2-sin(q1)*4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Inverse_jacobian[0][1] = ((cos(q1)*4.25E2-sin(q1+3.141592653589793/6.0)*8.5E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Inverse_jacobian[1][0] = ((sin(q1)*-4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Inverse_jacobian[1][1] = ((cos(q1)*4.25E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
desired_velocity[0][0] = vx;
desired_velocity[1][0] = vy;
Joint_velocity[0][0] = Inverse_jacobian[0][0]*desired_velocity[0][0] + Inverse_jacobian[0][1]*desired_velocity[1][0];
Joint_velocity[1][0] = Inverse_jacobian[1][0]*desired_velocity[0][0] + Inverse_jacobian[1][1]*desired_velocity[1][0];
// Integration
Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
Joint_1_position_prev = Joint_1_position;
Joint_2_position_prev = Joint_2_position;
Motor_1_position = Joint_1_position;
Motor_2_position = Joint_1_position + Joint_2_position;
}
void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald.
{ // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input
// Proportional part: // voor de motorsnelheden gebruikt.
theta_k_1= Kp * error_M1;
theta_k_2= Kp * error_M2;
// Integral part
error_integral_1 = error_integral_1+ error_M1*delta_t;
error_integral_2 = error_integral_2+ error_M2*delta_t;
u_i_1= Ki * error_integral_1;
u_i_2= Ki * error_integral_2;
// Sum all parts and return it
theta_t_1= theta_k_1 + u_i_1;
theta_t_2= theta_k_2 + u_i_2;
}
void Define_motor_dir() // Deze functie bepaalt de output richtingen die aan de motoren worden gegeven tijdens de operation mode.
{ // Wanneer de input (wordt ook als snelheid gegeven aan de motoren) positief is, wordt de draairichting van
if (theta_t_1 >= 0 && theta_t_2 >= 0) { // de motoren op nul gezet en vice versa. Dit betekent dat de motoren linksom draaien, gezien vanaf de assen.
motor1_dir.write(0);
motor2_dir.write(0);
}
if (theta_t_1 < 0 && theta_t_2 >= 0) {
motor1_dir.write(1);
motor1_dir.write(0);
}
if (theta_t_1 >= 0 && theta_t_2 < 0) {
motor1_dir.write(0);
motor2_dir.write(1);
} else {
motor1_dir.write(1);
motor2_dir.write(1);
}
}
void Controlling_system() // Hoofdfunctie die wordt toegepast tijdens de operation mode. Een gewenste snelheid in x- en y-richting wordt
{ // hiervoor opgesteld en is vervolgens de input voor de inverse kinematica functie. Door middel van een gewenste
Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de
// PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie
error_M1 = Motor_1_position + theta_h_1_rad; // aangeroepen die ervoor zorgt dat de motoren in de juiste richting gaan draaien.
error_M2 = Motor_2_position + theta_h_2_rad;
PI_controller();
abs_theta_t_1 = abs(theta_t_1);
abs_theta_t_2 = abs(theta_t_2);
motor1.write(abs_theta_t_1);
motor2.write(abs_theta_t_2);
Define_motor_dir();
}
void servo_horizontal() // Functie die ervoor zorgt dat de spatel horizontaal wordt gehouden door de servo motor in de operation mode.
{
q1_ser= q1*604.7887837;
q2_ser= q2*604.7887837;
theta_sout=theta_f+q1_ser+q2_ser;
if (theta_sout>=2400) {
theta_sout=2400;
}
if (theta_sout<=500) {
theta_sout=500;
mijnServo.SetPosition(theta_sout);
pc.printf("De servo staat op %f graden\n\r", theta_sout);
}
}
void servo_flip() // Functie die ervoor zorgt dat de spatel flipt, wanneer er een button wordt ingedrukt. Wordt eveneens gerealiseerd
{ // door de servo motor.
theta_sout=theta_f+q1_ser+q2_ser+400;
if (theta_sout>=2400) {
theta_sout=2400;
}
if (theta_sout<=500) {
theta_sout=500;
} else {
theta_sout;
}
mijnServo.SetPosition(theta_sout);
pc.printf("De servo staat op %f graden, in de klapstand\n\r", theta_sout);
}
// Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine
// meer tijd kosten dan wordt gegeven door de ticker. Dit zou mogelijk het encoder
// probleem kunnen verklaren. Indien er te weinig tijd is, zou de loop zichzelf in moeten
// halen. Start met een bool die true is, stel deze gelijk aan false in het begin van de loop
// en verander deze weer in true wanneer de hele loop voltooid is. In het geval dat de loop zichzelf
// inhaalt, blijft de bool false en wordt een string (There is a timing problem) geprint.
// RESULTAAT: de string wordt niet geprint, er zouden geen timing issues moeten zijn.
// Het script spreekt zichzelf dus tegen, experts komen ook niet uit dit probleem.
volatile bool loop_done = true;
void ProcessStateMachine(void) // Programmeren van de finite state machine.
{
if (!loop_done) {
pc.printf("There is a timing problem\r\n");
return;
}
loop_done = false;
// Berekenen van de motorhoeken (in radialen)
counts1 = Encoder1.getPulses(); // Verkrijgen van het aantal counts uit de encoders
counts2 = Encoder2.getPulses();
theta_h_1_rad = conversion_factor*(counts1-offset1); // Van het gemeten aantal counts wordt de offset afgetrokken, zodat het aantal counts in de referentie positie gelijk is aan 0.
theta_h_2_rad = conversion_factor*(counts2-offset2); // Vermenigvuldiging met de conversion factor zorgt ervoor dat het aantal counts wordt omgezet naar de huidige motorhoeken.
// Calculating joint angles based on motor angles (current encoder values)
q1 = theta_h_1_rad; // Relatieve hoek joint 1 in radialen.
q2 = theta_h_2_rad - theta_h_1_rad; // Relatieve hoek joint 2 in radialen.
// Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
// doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
// Vervolgens wordt de absolute waarde hiervan genomen
filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
// Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
// over het signaal gedaan
filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
// De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
scope.set(0, normalized_EMG_biceps_right);
scope.set(1, normalized_EMG_biceps_left);
scope.set(2, normalized_EMG_calf);
scope.send();
// Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
// later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
// 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
// waarna het gemiddelde wordt bepaald.
if (calib) {
EMG_calibration();
}
// Genormaliseerde EMG's berekenen
normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
// Finite state machine
switch (currentState) {
case Motors_off:
if (stateChanged) {
motors_off(); // Functie waarbij motoren uitgaan.
stateChanged = false;
pc.printf("Motors off state\r\n");
}
if (Emergency_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
emergency(); // Bij het indrukken van de emergency button, wordt overgegaan op de emergency functie.
}
if (Power_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
currentState = Calib_motor; // Er wordt doorgegaan naar de volgende state, wanneer de power button wordt ingedrukt.
stateChanged = true;
pc.printf("Moving to Calib_motor state\r\n");
}
break;
case Calib_motor: // In deze state wordt de robot arm met de hand in de juiste referentie (home) positie geplaatst.
// Wanneer dit is gebeurd, wordt de motor kalibratie knop ingedrukt. De offset wordt dan ingesteld als
if (stateChanged) { // het huidige aantal counts. Door deze offset in de komende metingen af te trekken van het aantal counts
pc.printf("Zet motoren handmatig in home positie\r\n"); // op dat moment, wordt ervoor gezorgd dat de motorhoeken in de referentiepositie gelijk zijn aan nul.
stateChanged = false;
}
if (Emergency_button_pressed.read() == false) {
emergency();
}
if (Motor_calib_button_pressed.read() == false) {
offset1 = counts1;
offset2 = counts2;
currentState = Calib_EMG;
stateChanged = true;
pc.printf("Moving to Calib_EMG state\r\n");
}
break;
case Calib_EMG: // In deze state wordt een kalibratie uitgevoerd van de EMG-signalen. Hiervoor wordt een bool (calib)
// samen met een teller (i_calib) in werking gezet, die ervoor zorgt dat de eerder beschreven EMG-kalibratie
if (stateChanged) { // wordt doorlopen. De spieren worden tijdens de kalibratie gedurende 5 seconden maximaal aangespannen.
i_calib = 0; // Na deze 5 seconden wordt doorgegaan naar de volgende state. De output van deze kalibratie is een gemiddelde
calib = true; // waarde van het EMG-signaal tijdens het maximaal aanspannen.
pc.printf("Span spieren aan\r\n");
stateChanged = false;
}
if (Emergency_button_pressed.read() == false) {
emergency();
}
if (i_calib > 2500) {
calib = false;
currentState = Homing;
stateChanged = true;
pc.printf("Moving to Homing state\r\n");
}
break;
case Homing: // In deze state wordt de homing uitgevoerd. De motoren draaien hier op een langzame snelheid, todat de gewenste referentiepositie
// wordt bereikt. In dit geval worden de motorsnelheden op 0 gezet. Indien beide motoren de juiste posities hebben bereikt, wordt
if (stateChanged) { // overgegaan naar de volgende state. De eerste keer dat de ProcessStateMachine in de homing state belandt, is dit al het geval,
stateChanged = false; // waardoor gelijk doorgegaan kan worden naar de operation mode.
}
if (Emergency_button_pressed.read() == false) {
emergency();
}
Homing_function();
if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0) {
currentState = Operation_mode;
stateChanged = true;
pc.printf("Moving to operation mode \r\n");
}
break;
case Operation_mode:
if (stateChanged) { // Aan het begin van de operation mode wordt aan allerlei variabelen een beginwaarde van 0 toegekend. Dit wordt één keer gedaan
motors_off(); // (stateChanged), waarna deze waarden verderop in deze state worden overschreven. In het geval dat er tussendoor teruggegaan is
Joint_1_position = 0; // naar een andere state, wordt op deze manier elke keer weer begonnen met de juiste beginwaarden.
Joint_2_position = 0;
Joint_1_position_prev = Joint_1_position;
Joint_2_position_prev = Joint_2_position;
Joint_velocity[0][0] = 0;
Joint_velocity[1][0] = 0;
Motor_1_position = 0;
Motor_2_position = 0;
theta_k_1 = 0.0;
theta_k_2 = 0.0;
error_integral_1 = 0.0;
error_integral_2 = 0.0;
stateChanged = false;
}
servo_horizontal(); // Zorgt ervoor dat de spatel horizontaal wordt gehouden.
if (servo_button_pressed.read()== false) {
servo_flip(); // Zorgt ervoor dat de spatel flipt, wanneer de servo knop wordt ingedrukt.
}
if (Power_button_pressed.read() == false) { // Wanneer de power button wordt ingedrukt, worden de motoren uitgezet.
motors_off();
currentState = Motors_off;
stateChanged = true;
pc.printf("Terug naar de state Motors_off\r\n");
}
if (Emergency_button_pressed.read() == false) { // Wanneer de emergency button wordt ingedrukt, wordt de emergency functie aangeroepen.
emergency();
}
if (Motor_calib_button_pressed.read() == false) { // Wanneer tijdens de operation mode teruggegaan moet worden naar de referentie positie, kan de button voor de motor kalibratie
motors_off(); // worden ingedrukt. In dit geval is het niet nodig om de motoren opnieuw te kalibreren, dus kan direct teruggegaan worden naar
currentState = Homing; // de state Homing. De motoren bewegen dan totdat de referentie positie weer wordt bereikt.
stateChanged = true;
pc.printf("Terug naar de state Homing\r\n");
}
if (normalized_EMG_biceps_right >= 0.3) { // Vanaf hier begint de daadwerkelijke besturing van de robot arm door middel van EMG signalen. Indien de rechter biceps wordt
// aangespannen, zal het genormaliseerde EMG signaal boven 0.3 uitkomen. Wanneer tegelijkertijd de kuit wordt aangespannen is er
if (normalized_EMG_calf < 0.3) { // een gewenste negatieve snelheid in de y-richting. Zo niet, dan is er een gewenste positieve snelheid in de y-richting.
vx = 0.0; // Deze gewenste snelheid wordt doorgevoerd in de functie Controlling_system, die onder andere inverse kinematics en een
vy = 0.02; // PI-controller bevat. Uiteindelijk wordt aan beide motoren een snelheid en richting toegekend, zodat de robot met de
} // gewenste snelheid in een rechte lijn verticaal kan bewegen.
if (normalized_EMG_calf >= 0.3) {
vx = 0.0;
vy = -0.02;
}
Controlling_system();
} else if (normalized_EMG_biceps_left >= 0.3) { // Indien de rechter biceps niet wordt aangespannen en de genormaliseerde EMG-waarde van de linker biceps groter is dan 0.3, geldt
if (normalized_EMG_calf < 0.3) { // hetzelfde als hierboven beschreven, maar dan voor de x-richting. Dit resulteert in een horizontale beweging in een rechte lijn.
vx = 0.02;
vy = 0.0;
}
if (normalized_EMG_calf >= 0.3) {
vx = -0.02;
vy = 0.0;
}
Controlling_system();
} else { // Indien beide biceps niet worden aangespannen, moet de robot arm niet bewegen. De snelheid in x- en y-richting moet dan gelijk zijn
vx = 0.0; // aan nul. Toch zal wel gecompenseerd moeten worden voor de zwaartekracht om de arm in dezelfde positie te houden. Deze snelheden
vy = 0.0; // worden daarom alsnog doorgevoerd in de functie Controlling_systeem. Dit resulteert in motorsnelheden en richtingen die ervoor zorgen
// dat de arm gebalanceerd wordt op de voorgaande positie.
Controlling_system();
}
break;
default:
motors_off(); // Hier wordt dezelfde functie als eerder toegepast om de motoren uit te schakelen -> safety!
pc.printf("Unknown or uninplemented state reached!\r\n");
}
loop_done = true; // Dit is nog een element voor het testen of de ProcessStateMachine zichzelf inhaalt. Indien de volledige
} // loop wel doorlopen is, verandert de bool hier in true. Hierdoor wordt de string aan het begin van de state machine
// NIET geprint. Zie een eerdere comment (vlak voor de definitie van de state machine) voor verdere toelichting.
int main(void)
{
pc.baud(115200); // De baud rate wordt op 115200 bits/sec gezet.
motor1.period_us(56); // Idealiter ligt deze frequentie tussen de 15 en 18 kHz. We hebben gekozen voor de kleinst mogelijke motor periode,
motor2.period_us(56); // die gelijk is aan 1/18000 = ongeveer 56 us.
pc.printf("Opstarten\r\n");
// Chain voor rechter biceps // De filterketens zijn apart voor elke spier gedefinieerd. Er wordt eerst een keten aangemaakt die bestaat uit een
bqcbr.add(&bqbr1).add(&bqbr2); // high-pass filter, gevolgd door een notch filter. Na het nemen van de absolute waarde van het bewerkte EMG-signaal
bqcbr2.add(&bqbr3).add(&bqbr4); // moet het tweede gedeelte van de filterketen worden toegepast. Hiervoor moeten twee low-pass filters aan elkaar
// Chain voor linker biceps // geketend worden.
bqcbl.add(&bqbl1).add(&bqbl2);
bqcbl2.add(&bqbl3).add(&bqbl4);
// Chain voor kuit
bqck.add(&bqk1).add(&bqk2);
bqck2.add(&bqk3).add(&bqk4);
loop_ticker.attach(&ProcessStateMachine, 0.002f); // We laten de ProcessStateMachine runnen met een frequentie van 500 Hz. Dit is gedaan om te kunnen matchen met de gewenste
// sampling frequentie voor EMG-signalen.
while(true) {
/* do nothing */
}
}