Wik de Haas
Aansturing robot groep 4 Biorobotics 2016. Beer bottle remover
Dependencies: Encoder HIDScope mbed
Fork of
Aansturing_knoppen
by 
Wik de Haas
Revision 4:9df6a1735cef, committed 2016-11-09
- Comitter:
- wikdehaas
- Date:
- Wed Nov 09 08:47:12 2016 +0000
- Parent:
- 3:57b98989b0b1
- Commit message:
- Program group 4. Beer bottle remover. Simplified for the cause of demonstration.
Changed in this revision
--- a/FilterDesign.cpp Thu Nov 03 11:33:29 2016 +0000
+++ b/FilterDesign.cpp Wed Nov 09 08:47:12 2016 +0000
@@ -13,36 +13,12 @@
const double b0_50 = 1.00000000000, b1_50 = -1.61803398875, b2_50 = 1.00000000000;
const double gain_50 = 0.95500000000;
- // Notch 100 Hz
- double v1_100 = 0, v2_100 = 0;
- const double a1_100 = -0.58713228931, a2_100 = 0.90250000000;
- const double b0_100 = 1.00000000000, b1_100 = -0.61803398875, b2_100 = 1.00000000000;
- const double gain_100 = 0.95200000000;
-
- // Notch 200 Hz
- double v1_200 = 0, v2_200 = 0;
- const double a1_200 = 1.53713228931, a2_200 = 0.90250000000;
- const double b0_200 = 1.00000000000, b1_200 = 1.61803398875, b2_200 = 1.00000000000;
- const double gain_200 = 0.95500000000;
-
- // High pass 10 Hz (oud)
+ // High pass 10 Hz
double v1_HP = 0, v2_HP = 0;
const double a1_HP = -1.91258127190, a2_HP = 0.91680625000;
const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
const double gain_HP = 0.95500000000;
- /*
- // High pass 50 Hz
- double v1_HP = 0, v2_HP = 0;
- const double a1_HP = -1.47413760026, a2_HP = 0.60062500000;
- const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
- const double gain_HP = 0.77000000000;
-
- // Low pass 0.7 Hz
- double v1_LP = 0, v2_LP = 0;
- const double a1_LP = -1.80000000000, a2_LP = 0.81000000000;
- const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
- const double gain_LP = 0.0025100000000;
- */
+
// Low pass 2 Hz
double v1_LP = 0, v2_LP = 0;
const double a1_LP = -1.99277224437, a2_LP = 0.99281296000;
@@ -53,8 +29,6 @@
{
double y_25 = Filter(u_E,v1_25,v2_25,a1_25,a2_25,b0_25,b1_25,b2_25,gain_25);
double y_50 = Filter(y_25,v1_50,v2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
- // double y_100 = Filter(y_50,v1_100,v2_100,a1_100,a2_100,b0_100,b1_100,b2_100,gain_100);
- //double y_200 = Filter(y_100,v1_200,v2_200,a1_200,a2_200,b0_200,b1_200,b2_200,gain_200);
double y_HP = Filter(y_50,v1_HP,v2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
double y_abs = fabs(y_HP);
double y_LP = Filter(y_abs,v1_LP,v2_LP,a1_LP,a2_LP,b0_LP,b1_LP,b2_LP,gain_LP);
--- a/FilterDesign2.cpp Thu Nov 03 11:33:29 2016 +0000
+++ b/FilterDesign2.cpp Wed Nov 09 08:47:12 2016 +0000
@@ -13,57 +13,12 @@
const double b0_25 = 1.00000000000, b1_25 = -1.90211303259, b2_25 = 1.00000000000;
const double gain_25 = 0.80000000000;
- // Notch 100 Hz
- double w1_100 = 0, w2_100 = 0;
- const double a1_100 = -0.58713228931, a2_100 = 0.90250000000;
- const double b0_100 = 1.00000000000, b1_100 = -0.61803398875, b2_100 = 1.00000000000;
- const double gain_100 = 0.95200000000;
-
- // Notch 200 Hz
- double w1_200 = 0, w2_200 = 0;
- const double a1_200 = 1.53713228931, a2_200 = 0.90250000000;
- const double b0_200 = 1.00000000000, b1_200 = 1.61803398875, b2_200 = 1.00000000000;
- const double gain_200 = 0.95500000000;
-
- // High pass 10 Hz (oud)
+ // High pass 10 Hz
double w1_HP = 0, w2_HP = 0;
const double a1_HP = -1.91258127190, a2_HP = 0.91680625000;
const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
const double gain_HP = 0.95500000000;
- /*
- // High pass 40 Hz
- double w1_HP = 0, w2_HP = 0;
- const double a1_HP = -1.51877533120, a2_HP = 0.60062500000;
- const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
- const double gain_HP = 0.78000000000;
- */
- /*
- // High pass 50 Hz
- double w1_HP = 0, w2_HP = 0;
- const double a1_HP = -1.47413760026, a2_HP = 0.60062500000;
- const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
- const double gain_HP = 0.77000000000;
- */
- /*
- // Low pass 11 Hz (oud)
- double w1_LP = 0, w2_LP = 0;
- const double a1_LP = -1.67736610515, a2_LP = 0.72250000000;
- const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
- const double gain_LP = 0.0120000000000;
-
- // Low pass 0.7 Hz
- double w1_LP = 0, w2_LP = 0;
- const double a1_LP = -1.80000000000, a2_LP = 0.81000000000;
- const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
- const double gain_LP = 0.0025100000000;
- */
- /*
- // Low pass 3 Hz
- double w1_LP = 0, w2_LP = 0;
- const double a1_LP = -1.70000000000, a2_LP = 0.72250000000;
- const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
- const double gain_LP = 0.0060000000000;
- */
+
// Low pass 2 Hz
double w1_LP = 0, w2_LP = 0;
const double a1_LP = -1.99277224437, a2_LP = 0.99281296000;
@@ -75,8 +30,6 @@
{
double y_25 = Filter2(u_E,w1_25,w2_25,a1_25,a2_25,b0_25,b1_25,b2_25,gain_25);
double y_50 = Filter2(y_25,w1_50,w2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
- // double y_100 = Filter2(u,w1_100,w2_100,a1_100,a2_100,b0_100,b1_100,b2_100,gain_100);
- // double y_200 = Filter2(y_100,w1_200,w2_200,a1_200,a2_200,b0_200,b1_200,b2_200,gain_200);
double y_HP = Filter2(y_50,w1_HP,w2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
double y_abs = fabs(y_HP);
double y_LP = Filter2(y_abs,w1_LP,w2_LP,a1_LP,a2_LP,b0_LP,b1_LP,b2_LP,gain_LP);
--- a/TY.cpp Thu Nov 03 11:33:29 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,135 +0,0 @@
-/*
-if(knop_1 == 0 && knop_2 == 0 && x<x_plek_T)
-
-//gegevens
-const double x_plek_T = 0.35, y_streep_TY_hoogte = 0.15;
-const double x_streep_half = 0.05, x_plek_Y = 0.45, y_midden_Y = 0.09;
-
-case 5: //Naar onderkant T
- while(x < x_plek_T)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x + tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar bovenkant T
- while(y < (y_base+y_streep_TY_hoogte))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar linkerkant bovenkant T
- while(x > (x_plek_T - x_streep_half))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x - tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar rechterkant bovenkant T
- while(x < (x_plek_T + x_streep_half))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x + tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden T
- while(x > x_plek_T)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x - tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar onderkant T
- while(y > y_base)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar onderkant Y
- while(x < x_plek_Y)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x + tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden Y
- while(y < (y_base+y_midden_Y))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar linksboven Y
- while(y < (y_base+y_streep_TY_hoogte))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x - tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden Y
- while(y > (y_base+y_midden_Y))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x + tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar rechtsboven Y
- while(y < (y_base+y_streep_TY_hoogte))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x + tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden Y
- while(y > (y_base+y_midden_Y))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x - tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar onderkant Y
- while(y > y_base)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar basis
- while(x > x_start)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x - tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- break;
- */
\ No newline at end of file
--- a/main.cpp Thu Nov 03 11:33:29 2016 +0000
+++ b/main.cpp Wed Nov 09 08:47:12 2016 +0000
@@ -2,7 +2,6 @@
#include "PID.h"
#include "encoder.h"
#include "math.h"
-#include "HIDScope.h"
#include "Filter.h"
#include "FilterDesign.h"
#include "Filter2.h"
@@ -20,16 +19,14 @@
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
+InterruptIn stop(SW3); //stoppen in case of emergency
+InterruptIn beginopnieuw(SW2); //reset het programma zonder op reset te drukken
Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
Encoder encoder_1(D13,D12);
Encoder encoder_2(D10,D9);
-HIDScope scope(4);
-
Ticker PID_ticker;
Ticker FILTER_ticker;
@@ -43,9 +40,6 @@
const double Vy = 0.05; //m/s
const double L2 = 0.35; //m
const double y_base = 0.045;
-//gegevens
-const double x_plek_T = 0.43, y_streep_TY_hoogte = 0.15;
-const double x_streep_half = 0.06, x_plek_Y = x_plek_T+0.13, y_midden_Y = 0.09;
//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
@@ -53,12 +47,10 @@
//controller
volatile double theta_1, theta_2, reference_1, plaats_1, error_1, pwm_1, reference_2, plaats_2, error_2, pwm_2;
//beginpositie
-volatile double x = x_start; //m
-volatile double y = y_base; //m
-volatile bool opgepakt = false;
-volatile bool zakpunt = false;
-volatile bool uitdrukken = false;
-volatile double y_oppakken = 0.135, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen
+volatile double x = x_start, y = y_base, positie_motor_1, positie_motor_2; //m
+//Voorwaarde voor terugbewegen
+volatile double y_oppakken = 0.135, y_stijgen = 0.15, x_zakken = 0.09;
+//voorwaarde dat programma aanstaat
volatile bool run_programma = true;
//PID
const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
@@ -91,11 +83,6 @@
if (pwm_2<0){motor_2 = 0;}
else {motor_2 = 1;}
pwm_motor_2 = fabs(pwm_2);
- scope.set(0,u_1);
- scope.set(1,y_1);
- scope.set(2,u_2);
- scope.set(3,y_2);
- scope.send();
}
//Ticker filterwaardes
void Filter_Samples()
@@ -114,15 +101,15 @@
pwm_motor_2 = 0;
run_programma = false;
}
-
+//Reset (niet nuttig tenzij HIDScope)
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;
+ positie_motor_1 = -1.5*theta_1;
+ 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);
@@ -156,28 +143,29 @@
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 && x>(x_start+x_zakken)){mode = 3;}
- else if (knop_1 == 0 && knop_2 == 0 && x<x_plek_T) {mode = 5;}
- 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
- */
+ else 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 && x>(x_start+x_zakken)){mode = 3;}
+ else {mode = 4;} //default
+
switch (mode)
{
+ //van de robot af
case 1: x = x + tijd_verschil*Vx;
if (x>0.6){x = 0.6;}
y = y_base;
break;
+ //naar de robot toe
case 2: x = x - tijd_verschil*Vx;
if (x<x_start){x = x_start;}
y = y_base;
break;
- case 3: while(y > (y_base-y_oppakken))
+ //routine for going back
+ case 3: //naar beneden
+ while(y > (y_base-y_oppakken))
{
tijd_nieuw = tijd;
tijd_verschil = tijd_nieuw - tijd_oud;
@@ -188,6 +176,7 @@
wait(0.05);
tijd_nieuw = tijd;
tijd_oud = tijd_nieuw;
+ //naar boven
while(y < (y_base+y_stijgen))
{
tijd_nieuw = tijd;
@@ -196,6 +185,7 @@
x = x;
tijd_oud = tijd_nieuw;
}
+ //naar de robot toe toe zakpositie
while(x > (x_start+x_zakken))
{
tijd_nieuw = tijd;
@@ -203,6 +193,7 @@
x = x-tijd_verschil*Vx*2;
tijd_oud = tijd_nieuw;
}
+ //naar uitdrukhoogte
while(y>y_base)
{
tijd_nieuw = tijd;
@@ -211,6 +202,7 @@
x = x;
tijd_oud = tijd_nieuw;
}
+ //uitdrukken
while(x>x_start-0.03)
{
tijd_nieuw = tijd;
@@ -219,7 +211,7 @@
x = x - tijd_verschil*Vx*2;
if (x<x_start)
{
- compensatie = 0.4;
+ compensatie = 0.3;
}
tijd_oud = tijd_nieuw;
}
@@ -228,133 +220,9 @@
x = x_start;
y = y_base;
break;
- case 5: //Naar onderkant T
- while(x < x_plek_T)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x + tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar bovenkant T
- while(y < (y_base+y_streep_TY_hoogte))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar linkerkant bovenkant T
- while(x > (x_plek_T - x_streep_half))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x - tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar rechterkant bovenkant T
- while(x < (x_plek_T + x_streep_half))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x + tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden T
- while(x > x_plek_T)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x - tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar onderkant T
- while(y > y_base)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar onderkant Y
- while(x < x_plek_Y)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x + tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden Y
- while(y < (y_base+y_midden_Y))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar linksboven Y
- while(y < (y_base+y_streep_TY_hoogte))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x - tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden Y
- while(y > (y_base+y_midden_Y))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x + tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar rechtsboven Y
- while(y < (y_base+y_streep_TY_hoogte))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y + tijd_verschil*Vy;
- x = x + tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar midden Y
- while(y > (y_base+y_midden_Y))
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x - tijd_verschil*Vx;
- tijd_oud = tijd_nieuw;
- }
- //Naar onderkant Y
- while(y > y_base)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- y = y - tijd_verschil*Vy;
- x = x;
- tijd_oud = tijd_nieuw;
- }
- //Naar basis
- while(x > x_start)
- {
- tijd_nieuw = tijd;
- tijd_verschil = tijd_nieuw - tijd_oud;
- x = x - tijd_verschil*Vx;
- y = y;
- tijd_oud = tijd_nieuw;
- }
+
break;
+ //doe niks
default: x = x;
y = y;
break;