Wik de Haas
Aansturing
Dependencies: Encoder mbed HIDScope
Diff: main.cpp
- Revision:
- 2:a42b34f9d6ab
- Parent:
- 1:a644028231b5
- Child:
- 3:57b98989b0b1
--- a/main.cpp Thu Oct 27 13:51:14 2016 +0000
+++ b/main.cpp Wed Nov 02 10:45:10 2016 +0000
@@ -3,17 +3,25 @@
#include "encoder.h"
#include "math.h"
#include "HIDScope.h"
+#include "Filter.h"
+#include "FilterDesign.h"
+#include "Filter2.h"
+#include "FilterDesign2.h"
//D8 doet het niet
DigitalIn knop_1(D2); //Motor 1
DigitalIn knop_2(D3); //Motor 2
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+
DigitalOut motor_1(D7); //richting (1 of 0)
PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
DigitalOut motor_2(D4); //richting (1 of 0)
PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
InterruptIn stop(SW3); //stoppen
+InterruptIn beginopnieuw(SW2); //stoppen
Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
@@ -23,18 +31,18 @@
HIDScope scope(4);
Ticker PID_ticker;
+Ticker FILTER_ticker;
Timer tijd;
//constante waardes
-const double x_start = 0.27;
+const double x_start = 0.255;
const double dt = 0.001;
+const double dt_f = 0.001;
const double Vx = 0.05; //m/s
const double Vy = 0.03; //m/s
-const double L1=0.30; //m
-const double L2=0.35; //m
-const double L3=0.13; //m
-const double y_base = 0.22;
+const double L2 = 0.35; //m
+const double y_base = 0.045;
//filter
volatile double tijd_oud, tijd_nieuw, tijd_verschil;
volatile double u_1, y_1, u_2, y_2; //ongefilerd signaal emg 1, gefilterd signaal emg 1, ongefilterd signaal emg 2, gefilterd signaal emg 2
@@ -44,24 +52,24 @@
//beginpositie
volatile double x = x_start; //m
volatile double y = y_base; //m
-volatile double yc= y + L3 - L1; //m
volatile bool opgepakt = false;
volatile bool zakpunt = false;
volatile bool uitdrukken = false;
-volatile double y_oppakken = 0.125, y_stijgen = 0.125, x_zakken = 0.1;
+volatile double y_oppakken = 0.10, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen
+volatile bool run_programma = true;
//PID
-const double m1_Kp = 20.0, m1_Ki = 0.5, m1_Kd = 0.6, m1_N = 50;
+const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
double m1_v1 = 0, m1_v2 = 0; // Memory variables
-const double m1_Ts = 0.01; // Controller sample time
-const double m2_Kp = 20.0, m2_Ki = 0.5, m2_Kd = 0.6, m2_N = 50;
+const double m1_Ts = 0.001; // Controller sample time
+const double m2_Kp = 2, m2_Ki = 5, m2_Kd = 0.1, m2_N = 50;
double m2_v1 = 0, m2_v2 = 0; // Memory variables
-const double m2_Ts = 0.01; // Controller sample time
+const double m2_Ts = 0.001; // Controller sample time
//Controller PID motors
void Controller()
{
- theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
+ theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
theta_2 = acos((x/L2)-cos(theta_1));
reference_1 = -1.5*theta_1; //reference
@@ -79,73 +87,108 @@
if (pwm_2<0){motor_2 = 0;}
else {motor_2 = 1;}
pwm_motor_2 = fabs(pwm_2);
- scope.set(0,pwm_1);
- scope.set(1,error_1);
- scope.set(2,pwm_2);
- scope.set(3,error_2);
+ scope.set(0,error_1);
+ scope.set(1,reference_1);
+ scope.set(2,pwm_1);
+ scope.set(3,plaats_1);
scope.send();
}
+//Ticker filterwaardes
+void Filter_Samples()
+{
+ u_1 = emg1.read();
+ y_1 = FilterDesign(u_1);
+ u_2 = emg2.read();
+ y_2 = FilterDesign2(u_2);
+}
//Failsave
void Stop() //Zet allebei de snelheden op nul
{
+ PID_ticker.detach();
pwm_motor_1 = 0;
pwm_motor_2 = 0;
- while(true){}
+ run_programma = false;
+}
+
+void Beginopnieuw()
+{
+ run_programma = true;
+ x = x_start; y = y_base; m1_v1 = 0; m1_v2 = 0; m2_v1 = 0; m2_v2 = 0;
+ theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
+ theta_2 = acos((x/L2)-cos(theta_1));
+ double positie_motor_1 = -1.5*theta_1;
+ double positie_motor_2 = -1.5*theta_2;
+ encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
+ encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
+ PID_ticker.attach(&Controller,dt);
+ tijd.reset();
+ tijd.start();
}
int main()
{
pc.baud(115200);
stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
- tijd.reset();
- tijd.start();
- theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
+ beginopnieuw.fall(&Beginopnieuw);
+ pwm_motor_1.period_us(1);
+ pwm_motor_2.period_us(1);
+ theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
theta_2 = acos((x/L2)-cos(theta_1));
double positie_motor_1 = -1.5*theta_1;
double positie_motor_2 = -1.5*theta_2;
encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
+ FILTER_ticker.attach(&Filter_Samples,dt_f);
PID_ticker.attach(&Controller,dt);
-
+ wait(1);
+ tijd.reset();
+ tijd.start();
while(true)
{
+ while(run_programma)
+ {
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
tijd_oud = tijd_nieuw;
if (knop_1 == 0 && knop_2 == 1){mode = 1;}
else if (knop_1 == 1 && knop_2 == 0){mode = 2;}
- else if (knop_1 == 0 && knop_2 == 0){mode = 3;}
- else {mode = 4;} //default
+ else if (knop_1 == 0 && knop_2 == 0 && x>(x_start+x_zakken)){mode = 3;}
+ else {mode = 4;} //default
+ /*
+ if (y_1 >= 0.4 && y_2 < 0.4) {mode = 1;}
+ else if (y_1 < 0.4 && y_2 >= 0.4) {mode = 2;}
+ else if (y_1 >= 0.4 && y_2 >= 0.4 && x>(x_start+x_zakken)){mode = 3;}
+ else {mode = 4;} //default
+ */
switch (mode)
{
case 1: x = x + tijd_verschil*Vx;
if (x>0.6){x = 0.6;}
y = y_base;
- yc= y + L3 - L1;
break;
case 2: x = x - tijd_verschil*Vx;
if (x<x_start){x = x_start;}
y = y_base;
- yc= y + L3 - L1;
break;
case 3: while(y > (y_base-y_oppakken))
{
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy*4;
- yc= y + L3 - L1;
+ y = y - tijd_verschil*Vy*3;
x = x;
tijd_oud = tijd_nieuw;
}
+ wait(1);
+ tijd_nieuw = tijd;
+ tijd_oud = tijd_nieuw;
while(y < (y_base+y_stijgen))
{
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy*2;
- yc= y + L3 - L1;
+ y = y + tijd_verschil*Vy*5;
x = x;
tijd_oud = tijd_nieuw;
}
@@ -153,36 +196,33 @@
{
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
- x = x-tijd_verschil*Vx;
- yc = yc;
+ x = x-tijd_verschil*Vx*2;
tijd_oud = tijd_nieuw;
}
while(y>y_base)
{
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- yc= y + L3 - L1;
+ y = y - tijd_verschil*Vy*2;
x = x;
tijd_oud = tijd_nieuw;
}
- while(x>x_start-0.04)
+ while(x>x_start-0.02)
{
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
y = y_base;
- yc= y + L3 - L1;
- x = x - tijd_verschil*Vx;
+ x = x - tijd_verschil*Vx*2;
tijd_oud = tijd_nieuw;
}
wait(0.5);
x = x_start;
y = y_base;
- yc= y + L3 - L1;
break;
default: x = x;
- yc = yc;
+ y = y;
break;
}
}
+ }
}
\ No newline at end of file