thijs ruikes
Robotcontrol groep 2
Dependencies: Encoder MODSERIAL mbed HIDScope
Diff: main.cpp
- Revision:
- 4:c22f3095b130
- Parent:
- 3:611fd72c9d46
- Child:
- 5:36df561f3ac1
--- a/main.cpp Sat Nov 01 19:40:46 2014 +0000
+++ b/main.cpp Sat Nov 01 21:00:25 2014 +0000
@@ -1,8 +1,21 @@
+//ROBOTCONTROL BMT M9 - GROEP 2
+//Alex Overbeek
+//Tom Baumeister
+//Bas van Buuren
+//Bas Mattern
+//Thijs ruikes
+
+/*Het script bestaat uit drie delen: INCLUDE AND DEFINE, FUNCTIONS, MAINSCRIPT. Per gedeelte wordt uitgelegd wat gedaan wordt.
+We laten de beweging in fases lopen. Fase 1 werkt in dit script. Voor fase 2 kan de positie al worden ingesteld, de slagbeweging
+moet nog gemaakt worden. Dit gaan we doen met een PID regelaar die naar een snelheid toe regelt.
+*/
+
+//INCLUDE AND DEFINE ALL
#include "mbed.h"
#include "MODSERIAL.h"
-
+#include <iostream>
+#include "encoder.h"
-#include "encoder.h"
#define K_P (0.1)
#define K_I (0.1)
#define K_D (0.0005 /TSAMP)
@@ -15,19 +28,17 @@
#define K_D1 (0.0001/TSAMP1)
#define TSAMP1 0.01
#define I_LIMIT1 1.
-// Alle constantes voor de filters definiëren
+// Constantes voor de filters definiëren:
// Constantes voor de Low Pass filter
#define A1LP 0.018180963222803
#define A0LP 0.016544013176248
#define B1LP -1.718913340044714
#define B0LP 0.753638316443765
-
// Constantes voor de High Pass Filter
#define A1HP -1.999801878951505
#define A0HP 0.999801878951505
#define B1HP -1.971717601075000
#define B0HP 0.972111984032897
-
// Constantes voor het Notch Filter
#define A0N 0.99436777112
#define A1N -1.89139989664
@@ -37,12 +48,11 @@
// Sample Time
#define TSAMP 0.001
+
// Outputs van de motor
Encoder encoderA(PTD5,PTA13);
Encoder encoder1(PTD0,PTD2);
-#include <iostream>
-
// Outputs naar de motor
PwmOut pwm(PTC8);
DigitalOut dir(PTC9);
@@ -50,23 +60,15 @@
DigitalOut dir1(PTA4);
//lampjes
-
DigitalOut rood(LED1);
DigitalOut blauw(LED3);
DigitalOut groen(LED2);
AnalogIn emg1(PTB1);
AnalogIn emg2(PTB2);
-// Vaststellen van het datatype van de outputs naar de motor
-float s_speed1;
-float s_dir1;
-float s_speed2;
-float s_dir2;
// PC communicatie
MODSERIAL pc(USBTX,USBRX);
-int set;
-
// Ticker voor de meetgegevens
Ticker timer;
volatile bool looptimerflag;
@@ -75,12 +77,8 @@
float emg_value2, ylp2, yhp2, yn2,ysum1 = 0, yave1=0;
float emg_value1, ylp1, yhp1, yn1,ysum2 = 0, yave2=0 ;
-
//variabvelen voor positie motor1
-float v1=0,out1 =0;
-int pos1 =0,zero =0, a=0;
-float out_i1 = 0;
-
+float v1=0,out_i1 = 0; //out_i1 globaal gedef om reset
// 0 of 1 waardes gedefinieerd uit het EMG, met 0 is te lage activiteit, 1 is hoog genoeg, voor de zekerheid toch als int.
uint8_t y1, y2;
@@ -93,37 +91,56 @@
// Teller voor hoeveel metingen er zijn gedaan
uint16_t teller=0;
-// Random meuk die ik uit batjespositie haal, nog ff opruimen
-int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
-float speed = 0.1, out =0;
-int pos =0;
-float v=0;
-float out_i = 0;
-int y;
+
+/*FUNCTIES
+In deze sectie worden de functie geprogrammeerd, hieronder een uitleg per functie. De functies zijn verdeeld in drie groepen. In het script
+staat achter enkele functies een 1 of een 2. Er worden 2 signalen bewerkt, dus sommige functies staan dubbel in het script. Signaal 1 wordt
+door de functies met 1, en signaal 2 wordt door de functie met 2, bewerkt.
+
+GLOBAL FUNCTIONS
+Clamp - "clampt" een waarde binnen grenzen, als de geclampte variabele over de grens heen gaat krijgt de variabele de grenswaarde.
+setLooptimerflag - Hiermee wordt de ticker aangeroepen.
+EMG FUNCTIONS
+readEMG - Leest het voltage af van de geselecteerde pin, in ons geval een EMG waarde.
+(notch/lp/hp)filter -Er worden drie filters gebruikt: lowpass, highpass en notchfilter. De funtie krijgt een input en geeft dan een gefilterde output.
+filter - Voert de notch,lp,hp filters achter elkaar uit op een input en geeft het gemiddelde van 500 meetpunten als output.
+threshold - Vergelijkt de input met een treshold, als de waarde boven de treshold komt (zodra een spier wordt aangespannen) wordt de output 1, anders 0.
+
+MOTORCONTROLFUNCTIONS
+resetarm - Brengt arm terug naar de nul positie, zet de encoder op 0.
+getv(delta_t) - Geeft de huidige draaisnelheid van motor1, delta_t is het tijdsinterval waarover gemeten wordt
+badje - Vergelijkt met dubbele input, y1 en y2. Er zijn vier situaties, beide = 0, y1=1, y2=1, beide =1.
+badposition - Leest de output van badje, laat het badje links, recht, niet draaien.
+pid... - Pid regelaar, de k waardes wordenin de define sectie gedefineerd.
+armposition - Op basis van de emgsignalen wordt hier een positie ingesteld.
+armtopos - Regelt de arm naar de opgegeven positie toe.
+*/
+
+//GLOBAL FUNCTIOMS
void clamp(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}
+void setlooptimerflag(void)
+{
+ looptimerflag = true;
+}
-// EMG 1 uitlezen
+//EMG FUNCTIONS
float readEMG1()
{
-
emg_value1=emg1.read();
return emg_value1;
}
-// EMG 2 uitlezen
float readEMG2()
{
-
emg_value2=emg2.read();
return emg_value2;
}
-// Notch Filter voor EMG 1
float notchfilter1(float ylp1)
{
static float yn1,x1=0,x2=0,y1=0,y2=0,x;
@@ -136,7 +153,6 @@
return yn1;
}
-// Notch Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
float notchfilter2(float ylp2)
{
static float x1=0,x2=0,y1=0,y2=0,x;
@@ -149,7 +165,6 @@
return yn2;
}
-// High Pass Filter voor EMG 1
float hpfilter1(float yn1)
{
static float x1=0,y1=0,x2=0, y2=0,x;
@@ -162,7 +177,6 @@
return yhp1;
}
-// High Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
float hpfilter2(float yn2)
{
static float x1=0,y1=0,x2=0, y2=0,x;
@@ -175,7 +189,6 @@
return yhp2;
}
-// Low Pass Filter voor EMG 1
float lpfilter1(float yhp1)
{
static float x1=0,y1=0,x2=0, y2=0,x;
@@ -188,7 +201,6 @@
return ylp1;
}
-// Low Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
float lpfilter2(float yhp2)
{
static float x1=0,y1=0,x2=0, y2=0,x;
@@ -201,13 +213,6 @@
return ylp2;
}
-// Stukje nodig voor de ticker
-void setlooptimerflag(void)
-{
- looptimerflag = true;
-}
-
-// Combinatie van de drie bovengenoemde filters voor EMG 1, en de middeling van 500 dataputen (nieuwe data 2 keer per seconde)
float filter1(float emg_value1)
{
static uint16_t n;
@@ -225,7 +230,6 @@
return yave1;
}
-// Idem voor EMG 2
float filter2(float emg_value2)
{
static uint16_t n;
@@ -244,7 +248,6 @@
return yave2;
}
-// Check of de waarde van het EMG boven de gedefinieerde threshold waarde uitkomt voor EMG 1
float threshold1 (float yave1)
{
if(yave1>THRESHOLD) {
@@ -255,7 +258,6 @@
return y1;
}
-//Idem voor EMG 2
float threshold2 (float yave2)
{
if(yave2>THRESHOLD) {
@@ -266,116 +268,36 @@
return y2;
}
-// Hoek van het badje vaststelen: begint met waarde 1, als EMG 1 alleen gebruikt wordt, gaat er 1 vanaf (maximaal 0), wordt EMG 2 alleen gebruikt, dan gaat er 1 bij (maximaal 2).
-// Als beide tegelijkertijd 2 maal achter elkaar gebruikt worden, dan wordt de hoek vastgezet (kan niet meer verandert worden tot de reset).
-
-
-// Stukje dat hetzelfde als bij de hoek van het badje regelt, maar dan voor de snelheid waarmee de arm moet bewegen. Heeft ook de waardes 0,1, of 2.
-// Bij 2 maal achter elkaar EMG 1 en EMG 2 gebruikt, wordt de snelheid vastgezet.
-
-void batposition(int y)
+//MOTORCONTROL FUNCTIONS
+float getv(float delta_t)
{
- switch(y) {
- case 0:
-
- dir = 1;
- pwm.write(0.4);
- wait(0.04);
- pwm.write(0);
- break;
- case 1:
-
- dir = 0;
- pwm.write(0.4);
- wait(0.04);
- pwm.write(0);
- break;
- case 2:
-
- pwm.write(0);
- break;
-
- }
-
-}
-float pid1(float setpoint, float measurement)
-{
- float error;
- static float prev_error = 0;
- float out_p = 0;
- float out_d = 0;
- error = setpoint-measurement;
- out_p = error*K_P1;
- out_i1 += error*K_I1;
- out_d = (error-prev_error)*K_D1;
- clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
- prev_error = error;
- out1 = out_i1+out_p+out_d;
- return out1;
-}
-
-float getv1(float delta_t)
-{
- int enca1,enca2,counts,enc;
+ int enca1=0,enca2=0,counts=0;
float v;
int n =0 ;
while(n<3) {
wait(delta_t);
- enc = encoder1.getPosition();
enca2 = enca1;
- enca1 = enc;
+ enca1 = encoder1.getPosition();
n++;
-
}
-
counts = (enca1 - enca2)/delta_t;
- v = (counts)*((2*3.14159265359)/1550);
+ v = (counts)*((2*3.14159265359)/1550)*0.5;
return v;
}
-float reset()
+
+float resetarm()
{
v1 = 1;
while(v1 !=0) {
dir1 = 0;
pwm1.write(0.1);
- v1 =getv1(0.1);
+ v1 =getv(0.1);
}
pwm1 = 0;
dir1 =1;
encoder1.setPosition(0);
-
return pwm1;
}
-uint8_t velocity (uint8_t y1, uint8_t y2)
-{
- static int stand=0;
- if (y1>0 && y2>0 && check>0) {
- speeddone=1;
- check=0;
- rood = 0;
- groen = 1;
- cout<<"ga naar mode 1"<<endl;
- } else if (y1>0 && y2>0) {
- check=1;
- } else if (y1>0) {
- if (speed>0) {
- stand=stand+1;
- check=0;
- cout<<"stand "<<stand<<endl;
- }
- } else if (y2>0 ) {
- stand=stand-1;
- check=0;
- cout<<"stand "<<stand<<endl;
- }
- else{
- stand = 3;
- check =0;
- }
-
- return stand;
-}
-
uint8_t badje (uint8_t y1, uint8_t y2)
{
if (y1>0 && y2>0 && check>0) {
@@ -384,7 +306,6 @@
rood = 1;
groen = 0;
cout<<"ga naar mode 2"<<endl;
-
} else if (y1>0 && y2>0) {
check=1;
} else if (y1>0) {
@@ -393,21 +314,86 @@
} else if (y2>0 ) {
badjes=1;
check=0;
- }
-
- else {
+ } else {
check=0;
badjes = 2;
}
return badjes;
}
-void gotopos(int pos1)
+void batposition(int y)
+{
+ switch(y) {
+ case 0:
+
+ dir = 1;
+ pwm.write(0.4);
+ wait(0.04);
+ pwm.write(0);
+ break;
+ case 1:
+ dir = 0;
+ pwm.write(0.4);
+ wait(0.04);
+ pwm.write(0);
+ break;
+ case 2:
+ pwm.write(0);
+ break;
+
+ }
+
+}
+
+float pidposition(float setpoint, float measurement)
{
- int place;
+ float error, out, out_p=0,out_d=0;
+ static float prev_error = 0;
+ error = setpoint-measurement;
+ out_p = error*K_P1;
+ out_i1 += error*K_I1;
+ out_d = (error-prev_error)*K_D1;
+ clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
+ prev_error = error;
+ out = out_i1+out_p+out_d;
+ return out;
+}
+
+
+
+uint8_t armposition (uint8_t y1, uint8_t y2)
+{
+ static int stand=0;
+ if (y1>0 && y2>0 && check>0) {
+ speeddone=1;
+ check=0;
+ rood = 0;
+ groen = 1;
+ cout<<"ga naar mode 1"<<endl;
+ } else if (y1>0 && y2>0) {
+ check=1;
+ } else if (y1>0) {
+ stand=stand+1;
+ check=0;
+ cout<<"stand "<<stand<<endl;
+ } else if (y2>0 ) {
+ stand=stand-1;
+ check=0;
+ cout<<"stand "<<stand<<endl;
+ } else {
+ stand = 3;
+ check =0;
+ }
+
+ return stand;
+}
+
+void armtopos(int pos1)
+{
+ int place=0;
switch(pos1) {
case 0:
- place = pos1;
+ place = pos1;
break;
case 1:
place = 100;
@@ -421,95 +407,76 @@
place = 250;
break;
}
-
+
while((abs(place-encoder1.getPosition()) >6)|| (v1!= 0)) {
while(!looptimerflag);
looptimerflag = false;
- out1 = pid1(place,encoder1.getPosition());
+ pwm1 = pidposition(place,encoder1.getPosition());
- if(out1>0) {
+ if(pwm1>0) {
dir1 = 1;
- } else if(out1<0) {
+ } else if(pwm1<0) {
dir1 = 0;
}
- pwm1 = fabs(out1);
- v1 = getv1(0.001);
+ pwm1 = fabs(pwm1);
+ v1 = getv(0.001);
}
pwm1 =0;
cout<<"place done"<<endl;
}
-// Main code
+// MAIN SCRIPT
int main()
{
- reset();
rood = 0;
blauw = 1;
groen = 1;
- // Startcondities
+ resetarm();
pc.baud(115200);
timer.attach(setlooptimerflag,TSAMP);
- // 20 secondes wachten, zodat er geen effecten van het opstarten van het bordje, alvast waardes voor het badje of de arm vastzetten.
wait(2);
- cout<<"fase1"<<endl;
while(1) {
-
- // Ticker
+ //Per TSAMP word EMG uitgelzen, gefilterd en gemiddeld
while(!looptimerflag);
-
looptimerflag = false;
-
- // EMG uitlezen
emg_value1 = readEMG1();
emg_value2 = readEMG2();
-
- // Filters gebruiken
yave1 = filter1(emg_value1);
yave2 = filter2(emg_value2);
-
- // Checken of de waardes boven de threshold uitkomen
y1=threshold1(yave1);
y2=threshold2(yave2);
-
- // Aantal metingen tellen
teller++;
- // Om de 500 metingen controleren
- if (teller==500) {
+ /*Dit gedeelte voert de bewegingen uit. In de eerste fase kan met de linker spier het batje naar links gedraaid worden,
+ en met de rechter spier het batje naar recht. Als het badje in de juiste positie staat kunnen beide spieren tegelijk
+ aangespannen worden om naar de volgende fase te gaan. In fase 2 wordt de armpositie ingesteld. Hoe groter de afstand van de
+ arm met de bal, hoe harder de arm gaat slaan. Zodra de arm in de goede positie staat kan door beide spieren aan te spannen worden
+ doorgegaan naar fase 3. In fase 3 slaat de arm de bal. De snelheid die ingesteld wordt waarmee de arm gaat slaan wordt bepaald aan de
+ hand van de positie waarin de arm in fase 2 is gezet. Zodra de arm de bal geslagen heeft moet de arm gereset worden en terug gaan naar
+ fase 1.*/
+ if (teller==500) {
teller=0;
- // Als de hoek voor het badje nog niet klaar is, zet badje in de huidig aangegeven stand
if (badjedone==0) {
badjestand=badje(y1,y2);
-
batposition(badjestand);
-
- // Als de hoek voor het badje wel klaar is:
} else if (badjedone==1) {
- // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt
if (speeddone==1) {
armspeed=armstand+1;
badjedone=0;
speeddone=0;
badjestand=1;
armstand=0;
- // Anders, pas de hoek van de arm aan naar de stand die hoort bij de huidig geselecteerde snelheid
- } else {
- armstand=velocity(y1,y2);
-
+ } else {
+ armstand=armposition(y1,y2);
cout<<"armstand "<<armstand<<endl;
- gotopos(armstand);
+ armtopos(armstand);
}
-
}
}
- // Verzend data naar de scopes
-
-
-
}
}
\ No newline at end of file