Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 29:8e0a7c33e4e7
- Parent:
- 28:7c7508bdb21f
- Child:
- 30:0a328a9a4788
--- a/main.cpp Tue Oct 29 14:14:15 2019 +0000
+++ b/main.cpp Tue Oct 29 18:24:42 2019 +0000
@@ -8,6 +8,7 @@
#include <math.h>
#include "Servo.h"
#include <cmath>
+#include <complex>
Serial pc(USBTX, USBRX);
@@ -112,11 +113,39 @@
double mean_EMG_biceps_left;
double mean_EMG_calf;
-// VARIABELEN VOOR OPERATION MODE
+// VARIABELEN VOOR OPERATION MODE (EMG)
double normalized_EMG_biceps_right;
double normalized_EMG_biceps_left;
double normalized_EMG_calf;
+// VARIABELEN VOOR OPERATION MODE (RKI)
+double vx; // Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
+double vy; // Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
+
+const double delta_t = 0.002;
+
+double Joint_1_position_x;
+double Joint_2_position_x;
+
+double Joint_velocity_x[2][1];
+double Joint_velocity_y[2][1];
+
+double q1_dot_x;
+double q2_dot_x;
+
+double q1_dot_y;
+double q2_dot_y;
+
+double q1;
+double q2;
+
+double Motor_1_velocity_x;
+double Motor_2_velocity_x;
+
+double Motor_1_velocity_y;
+double Motor_2_velocity_y;
+
+
// VOIDS
// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
@@ -133,7 +162,9 @@
void motors_off()
{
motor1.write(0);
+ motor1_dir.write(1);
motor2.write(0);
+ motor2_dir.write(1);
pc.printf("Motoren uit functie\r\n");
}
@@ -147,6 +178,36 @@
pc.printf("Motoren aan functie\r\n");
}
+void Inverse_Kinematics()
+{
+ // Defining joint velocities based om calculations of matlab
+
+ Joint_velocity_x[0][0] = (vx*(exp(q2*sqrt(-1.0))*(-1.798599718146044E18+6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(-1.798599718146044E18-6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(1.168095770314175E18+1.321169505615574E18*sqrt(-1.0))+exp(q1*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*3.828059683264922E18+1.168095770314175E18-1.321169505615574E18*sqrt(-1.0))*(2.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+ Joint_velocity_x[1][0] = (vx*(exp(q2*sqrt(-1.0))*(1.798599718146044E18-6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(1.798599718146044E18+6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(-1.168095770314175E18-1.321169505615574E18*sqrt(-1.0))+-1.168095770314175E18+1.321169505615574E18*sqrt(-1.0))*(2.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+
+ Joint_velocity_y[0][0] = (vy*(exp(q2*sqrt(-1.0))*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*1.914029841632461E18+6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))*(4.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+ Joint_velocity_y[1][0] = (vy*(exp(q2*sqrt(-1.0))*(3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(-6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+-6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))*(4.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+
+ q1_dot_x = real(Joint_velocity_x[0][0]);
+ q2_dot_x = real(Joint_velocity_x[1][0]);
+
+ q1_dot_y = real(Joint_velocity_y[0][0]);
+ q2_dot_y = real(Joint_velocity_y[1][0]);
+
+ // Integratie
+ Joint_1_position_x = Joint_1_position_x + Joint_velocity_x[0][0]*delta_t;
+ Joint_2_position_x = Joint_2_position_x + Joint_velocity_x[1][0]*delta_t;
+
+ Joint1_position_y = Joint_1_position_y + Joint_velocity_y[0][0]*delta_t;
+ Joint2_position_y = Joint_2_position_y + Joint_velocity_y[1][0]*delta_t;
+
+ Motor_1_position_x = Joint_position_x[0][0];
+ Motor_2_position_x = Joint_position_x[0][0] + Joint_position_x[1][0];
+
+ Motor_1_position_y = Joint_position_y[0][0];
+ Motor_2_position_y = Joint_position_y[0][0] + Joint_position_y[1][0];
+}
+
// Finite state machine programming (calibration servo motor?)
void ProcessStateMachine(void)
{
@@ -158,6 +219,10 @@
theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
+ // Calculating joint angles based on motor angles (current encoder values)
+ q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
+ q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
+
// 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());
@@ -221,20 +286,27 @@
stateChanged = false;
pc.printf("Motors off state\r\n");
}
+ if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
+ emergency();
+ }
if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- motors_on();
currentState = Calib_motor;
stateChanged = true;
pc.printf("Moving to Calib_motor state\r\n");
}
- if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- emergency();
- }
break;
case Calib_motor:
- if (stateChanged && Motor_calib_button_pressed.read() == false) {
+ if (stateChanged) {
+ pc.printf("Zet motoren handmatig in home positie\r\n");
+ stateChanged = false;
+ }
+
+ if (Emergency_button_pressed.read() == false) {
+ emergency();
+ }
+ if (Motor_calib_button_pressed.read() == false) {
theta_h_1_rad = 0;
theta_h_2_rad = 0;
pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad);
@@ -242,31 +314,27 @@
stateChanged = true;
pc.printf("Moving to Calib_EMG state\r\n");
}
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
break;
case Calib_EMG:
if (stateChanged) {
- motors_off();
i_calib = 0;
calib = true;
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");
}
-
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
break;
case Homing: // NOG NAAR KIJKEN
@@ -275,87 +343,84 @@
// Ervoor zorgen dat de motoren zo bewegen dat de robotarm
// (inclusief de end-effector) in de juiste home positie wordt gezet
motors_on();
-
stateChanged = false;
- motors_off();
- pc.printf("Moving to operation mode \r\n");
}
if (Emergency_button_pressed.read() == false) {
emergency();
}
-
- //...
-
- if (homing done)
+
+ // Hier moet iets van encoder counts waarde maal -1 worden gedaan om
+ // naar positie 0 te bewegen
+
+ if (Power_button_pressed.read() == false)
+ //if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0)
+ // Veranderen wanneer encoders werken en motor in elkaar zit
{
currentState = Operation_mode;
stateChanged = true;
+ pc.printf("Moving to operation mode \r\n");
}
-
+
break;
case Operation_mode: // Overgaan tot emergency wanneer referentie niet
// overeenkomt met werkelijkheid
// pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
- if (stateChanged)
-
- // Hier moet een functie worden aangeroepen die ervoor zorgt dat
- // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
- // zodat de robotarm kan bewegen
+ if (stateChanged) {
+ motors_off();
+ Joint_velocity_x[0][0] = 0;
+ Joint_velocity_x[1][0] = 0;
+ Joint_velocity_y[0][0] = 0;
+ Joint_velocity_y[1][0] = 0;
+ stateChanged = false;
+ }
+ // Hier moet een functie worden aangeroepen die ervoor zorgt dat
+ // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
+ // zodat de robotarm kan bewegen
- {
- if (normalized_EMG_biceps_right >= 0.3) {
- motor1.write(0.3);
- motor2.write(0.3);
- motor1_dir.write(0);
- motor2_dir.write(0);
- //if (normalized_EMG_calf >= 0.3)
- //{
- //motor1.write(0.1);
- //motor1_dir = !motor1_dir;
- //}
+ // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
+ // motors_off();
+ // currentState = Motors_off;
+ // stateChanged = true;
+ // pc.printf("Terug naar de state Motors_off\r\n");
+ // }
+ if (Emergency_button_pressed.read() == false) {
+ emergency();
+ }
+ if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
+ motors_off();
+ currentState = Homing;
+ stateChanged = true;
+ pc.printf("Terug naar de state Homing\r\n");
+ }
+ if (normalized_EMG_biceps_right >= 0.3) {
+ motor1.write(0.3);
+ motor2.write(0.3);
+ motor1_dir.write(0);
+ motor2_dir.write(0);
+ //if (normalized_EMG_calf >= 0.3)
+ //{
+ //motor1.write(0.1);
+ //motor1_dir = !motor1_dir;
+ //}
- } else if (normalized_EMG_biceps_left >= 0.3) {
- motor2.write(0.9);
- motor1.write(0.9);
- motor1_dir.write(1);
- motor2_dir.write(1);
- //if (normalized_EMG_calf >= 0.3)
- //{
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- //}
- } else {
- motor1.write(0);
- motor2.write(0);
- }
+ } else if (normalized_EMG_biceps_left >= 0.3) {
+ motor2.write(0.9);
+ motor1.write(0.9);
+ motor1_dir.write(1);
+ motor2_dir.write(1);
//if (normalized_EMG_calf >= 0.3)
//{
// motor1_dir = !motor1_dir;
// pc.printf("Richting zou om moeten draaien");
// motor2_dir = !motor2_dir;
//}
-
+ } else {
+ motor1.write(0);
+ motor2.write(0);
}
- if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- motors_off();
- currentState = Motors_off;
- stateChanged = true;
- pc.printf("Terug naar de state Motors_off\r\n");
- }
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
- // wait(25);
- // else
- // {
- // currentState = Homing;
- // stateChanged = true;
- // pc.printf("Terug naar de state Homing\r\n");
- // }
break;
default: