Biorobotics 2019 groep 24
Calibratie motor, demo, (EMG calibratie en movement werkt niet)
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM
Diff: main.cpp
- Revision:
- 5:810892d669f9
- Parent:
- 4:7d0df890e801
- Child:
- 6:91cdad4e00e8
--- a/main.cpp Wed Oct 30 16:07:58 2019 +0000
+++ b/main.cpp Thu Oct 31 15:56:07 2019 +0000
@@ -3,7 +3,6 @@
#include "MODSERIAL.h"
#include "QEI.h"
#include "FastPWM.h"
-#include "encoder.h"
const double PI = 3.1415926535897;
@@ -11,42 +10,45 @@
InterruptIn but2(D0);
DigitalIn butsw2(SW3);
DigitalIn butsw3(SW2);
-AnalogIn potMeter1(A2);
-AnalogIn potMeter2(A1);
+AnalogIn potMeter1(A5);
+AnalogIn potMeter2(A4);
-DigitalOut motor1_direction(D4); //value 0=CCW, 1=CW
-FastPWM motor1_pwm(D5); //Biorobotics Motor 1 PWM controle van de snelheid
-DigitalOut motor2_direction(D7); //value 0=CCW, 1=CW
-FastPWM motor2_pwm(D6);
+DigitalOut motor1_direction(D4); //richting van motor1
+FastPWM motor1_pwm(D5); //Motor 1 PWM controle van de snelheid
+DigitalOut motor2_direction(D7); //richting van motor2
+FastPWM motor2_pwm(D6); //Motor 2 PWM controle van de snelheid
Ticker loop_ticker;
// SERIAL COMMUNICATION WITH PC.................................................
Serial pc(USBTX, USBRX);
-enum States {s_idle, s_calibratie_encoder, s_demo};
+enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibratie};
States state;
// ENCODER .....................................................................
QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
QEI enc2 (D9, D8, NC, 4200); //encoder 2 gebruiken
-float dt = 0.001;
+const float dt = 0.001;
double e_int = 0;
double e_int2 = 0;
-float theta1;
-float theta2;
-int enc1_zero = 0;//the zero position of the encoders, to be determined from the
-int enc2_zero = 0;//encoder calibration
+double theta1;
+double theta2;
+int enc1_zero = 0;
+int enc2_zero = 0;
int enc1_value;
int enc2_value;
-volatile double Huidigepositie1;
-volatile double Huidigepositie2;
+const int maxwaarde = 400;
+
+volatile double Huidigepositie1=0;
+volatile double Huidigepositie2=0;
volatile double motorValue1;
volatile double motorValue2;
volatile double refP;
volatile double refP2;
volatile double error1;
volatile double error2;
+
bool state_changed = false;
volatile bool A=true;
volatile bool B=true;
@@ -60,35 +62,63 @@
volatile bool direction1 = clockwise;
volatile bool direction2 = clockwise;
+// RKI VARIABELEN...............................................................
+// Lengtes zijn in meters
+// Vaste variabelen:
+const double L0 = 0.30; // lengte arm 1 --> moet nog goed worden opgemeten!
+const double L2 = 0.30; // Lengte arm 2 --> moet ook nog goed opgemeten worden!
+const double r_trans = 0.035; // gebruiken voor translation naar shaft rotation
+
+// Variërende variabelen:
+double q1 = 0; // Motorhoek van joint 1 op beginpositie
+double q2 = PI/2; // Motorhoek van joint 2 op beginpositie
+double v_x; // Snelheid van end effector in x richting --> Door EMG signals
+double v_y; // Snelheid van end effector in y richting --> Door EMG signals
+
+double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
+double Cq2; // Joint angle van het systeem (gecorrigeerd voor gear ratio 1:1.1)
+
+double q1_dot; // Benodigde hoeksnelheid van motor 1
+double q2_dot; // Benodigde hoeksnelheid van motor 2
+
+double q1_ii; // Referentie signaal voor motorhoek q1
+double q2_ii; // Referentie signaal voor motorhoek q2
+
+//PI VARIABELEN................................................................
+const double kp=0.002; // Soort van geschaald --> meer onderzoek nodig
+const double ki=0.0001;
+const double kp2=0.002;
+const double ki2=0.0001;
+
void rest()
- //Idle state. Used in the beginning, before the calibration states.
-{
- if (but1_pressed) {
+ // Rust. Hier wordt niets uitgevoerd. Wanneer button1
+{ // wordt ingedrukt gaan we naar de volgende state waar
+ if (but1_pressed) { // de encoders worden gekalibreerd.
state_changed = true;
state = s_calibratie_encoder;
but1_pressed = false;
}
}
-void calibratie_encoder()
-{
- if (state_changed) {
- pc.printf("Started encoder calibration\r\n");
+void calibratie_encoder() // calibratie encoder. Hier worden de encoders gekalibreerd en op
+{ // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de
+ if (state_changed) { // demo modus, wanneer op button 2 wordt gedruk gaan we naar
+ pc.printf("Started encoder calibration\r\n"); // de EMG calibratie
state_changed = false;
}
if (but1_pressed) {
pc.printf("Encoder has been calibrated \r\n");
enc1_value = enc1.getPulses();
enc2_value = enc2.getPulses();
- //enc1_value -= enc1_zero;
- //enc2_value -= enc2_zero;
+ //enc1_value -= enc1_zero;
+ //enc2_value -= enc2_zero;
theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
enc1_zero = enc1_value;
enc2_zero = enc2_value;
but1_pressed = false;
state_changed = true;
- state = s_demo;
+ state = s_demonstratie;
pc.printf("enc01: %i\r\n", enc1_zero);
pc.printf("enc1value: %i\r\n", enc1_value);
pc.printf("enc02: %i\r\n",enc2_zero);
@@ -97,37 +127,75 @@
pc.printf("hoek 2: %f\r\n",theta2);
}
- if (but2_pressed) {
+ if (but2_pressed) {
+ pc.printf("Encoder has been calibrated \r\n");
+ enc1_value = enc1.getPulses();
+ enc2_value = enc2.getPulses();
+ //enc1_value -= enc1_zero;
+ //enc2_value -= enc2_zero;
+ theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
+ theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
+ enc1_zero = enc1_value;
+ enc2_zero = enc2_value;
+ but2_pressed = false;
state_changed = true;
- state = s_idle;
- but2_pressed = false;
- pc.printf("FAILURE!\r\n");
+ state = s_EMGcalibratie;
+ pc.printf("enc01: %i\r\n", enc1_zero);
+ pc.printf("enc1value: %i\r\n", enc1_value);
+ pc.printf("enc02: %i\r\n",enc2_zero);
+ pc.printf("enc2value: %i\r\n", enc2_value);
+ pc.printf("hoek 1: %f\r\n",theta1);
+ pc.printf("hoek 2: %f\r\n",theta2);
+ }
+}
+
+void demonstratie()
+{
+ if (state_changed) { //
+ pc.printf("Demonstratie gestart\r\n") ; //
state_changed = false;
}
+ }
+void EMGcalibratie()
+{
+
+}
+
+void RKI()
+{
+ Lq1 = q1*r_trans;
+ Cq2 = q2/1.1; //1.1 is gear ratio
+
+ q1_dot = (v_x + (v_y*(/*L1+*/L2*sin(q2/1.1)))/(L0 + q1*r_trans + L2*cos(q2/1.1)))/r_trans;
+ q2_dot = (1.1*v_y)/(L0 + q1*r_trans + L2*cos(q2/1.1));
+
+ q1_ii = q1 + q1_dot*dt;
+ q2_ii = q2 + q2_dot*dt;
+
+ q1 = q1_ii;
+ q2 = q2_ii;
+
}
double GetReferencePosition()
{
- double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
- int maxwaarde = 400; //
+ double Potmeterwaarde = potMeter2.read();
+
double refP = Potmeterwaarde*maxwaarde;
- return refP; //
+ return refP;
}
double GetReferencePosition2()
{
- double potmeterwaarde2 = potMeter1.read();
- int maxwaarde2 = 400; //
- double refP2 = potmeterwaarde2*maxwaarde2;
- return refP2; //
+ double potmeterwaarde2 = potMeter1.read();
+ double refP2 = potmeterwaarde2*maxwaarde;
+ return refP2;
}
-
-double FeedBackControl(double error, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+
+double FeedBackControl(double error, double &e_int)
{
- double kp = 0.0015; // kind of scaled.
double Proportional= kp*error1;
-
- double ki = 0.0001; // kind of scaled.
+
e_int = e_int+dt*error1;
double Integrator = ki*e_int;
@@ -135,12 +203,10 @@
return motorValue1;
}
-double FeedBackControl2(double error2, double &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
-{
- double kp2 = 0.002; // kind of scaled.
+double FeedBackControl2(double error2, double &e_int2)
+{
double Proportional2= kp2*error2;
-
- double ki2 = 0.00005; // kind of scaled.
+
e_int2 = e_int2+dt*error2;
double Integrator2 = ki2*e_int2;
@@ -148,25 +214,23 @@
return motorValue2;
}
+
void SetMotor1(float motorValue1) {
- // Given motorValue1<=1, writes the velocity to the pwm control.
- // MotorValues outside range are truncated to within range.
+ // gegeven motorValue1 <=1, schrijft snelheid naar pwm.
+ // MotorValues buiten range worden afgekapt dat ze binnen de range vallen.
motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
}
void SetMotor2(float motorValue2) {
- // Given motorValue2<=1, writes the velocity to the pwm control.
- // MotorValues outside range are truncated to within range.
motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
}
-void MeasureAndControl(void)
+void MeasureAndControl()
{
- //SetpointRobot();
// RKI aanroepen
//RKI();
// hier the control of the 1st control system
- refP = GetReferencePosition(); //moet nog met RKI
+ refP = GetReferencePosition(); //moet eigenlijk nog met RKI
Huidigepositie1 = enc1.getPulses();
error1 = (refP - Huidigepositie1);// make an error
motorValue1 = FeedBackControl(error1, e_int);
@@ -177,7 +241,7 @@
error2 = (refP2 - Huidigepositie2);// make an error
motorValue2 = FeedBackControl2(error2, e_int2);
SetMotor2(motorValue2);
- pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
+ pc.printf("refP = %d, huidigepos = %d, motorvalue = %d, refP2 = %d, huidigepos2 = %d, motorvalue2 = %d \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
}
@@ -185,7 +249,7 @@
{
if (butsw2==0) {
if (A==true){// zodat het knopje 1 x wordt afgelezen
- // reverses the direction
+ // richting veranderen
motor1_direction.write(direction1 = !direction1);
pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
A=false;
@@ -203,34 +267,34 @@
}
}
else{
- B=true;
-}
+ B=true;}
+
+
}
-
-/*void measure_signals()
-{
- enc1_value = enc1.getPulses();
- enc2_value = enc2.getPulses();
- enc1_value -= enc1_zero;
- //enc2_value -= enc2_zero;
- theta1 = (float)(enc1_value)/(float)(8400)*2*PI;
- theta2 = (float)(enc2_value)/(float)(8400)*2*PI;
- }
-*/
+
void state_machine()
{
//run current state
switch (state) {
- case s_idle:
+ case s_idle: // in deze state gebeurd niets. Als op knop 1 wordt gedrukt
+ rest(); // gaan we over naar s_calibratie_encoder
+ break;
+ case s_calibratie_encoder: // in deze state worden de encoders gekalibreerd. knop 1 -> s_demonstratie
+ calibratie_encoder(); // knop 2 -> s_EMGcalibratie
+ break;
+ case s_demonstratie: // in deze state kunnen de motors worden bestuurd met de potmeters
+ MeasureAndControl(); // en switch 2 en 3( pot1,sw2->motor1 / pot2,sw3->motor2
+ direction(); // als op knop 2 wordt gedrukt komen we in de s_idle state
+ if (but2_pressed) {
+ pc.printf("fail. \r\n");
+ but2_pressed = false;
+ state_changed = true;
+ state = s_idle;
+ }
+ break;
+ case s_EMGcalibratie:
rest();
break;
- case s_calibratie_encoder:
- calibratie_encoder();
- break;
- case s_demo:
- MeasureAndControl();
- direction();
- break;
}
}
@@ -255,7 +319,6 @@
void main_loop()
{
- //measure_signals();
state_machine();
}
@@ -270,6 +333,7 @@
but2.fall(&but2_interrupt);
loop_ticker.attach(&main_loop, dt);
pc.printf("main_loop is running\n\r");
-
- }
+
+}
+