Gerjan Wolterink
Broncode projectgroep 2 Belinda Brandwacht s1226290 Esther Keulers s1255444 Maaike Schotman s1274104 Gerjan Wolterink s1197797 Roelof van Zwol s1240811
Dependencies: Encoder MODSERIAL mbed
Fork of
Begintotaalscript
by 
Esther Keulers
Revision 7:5380d17310e8, committed 2013-11-06
- Comitter:
- gerjan
- Date:
- Wed Nov 06 17:02:58 2013 +0000
- Parent:
- 6:d4f355d72f66
- Commit message:
- Broncode projectgroep 2; ; Belinda Brandwacht s1226290; Esther Keulers s1255444; Maaike Schotman s1274104; Gerjan Wolterink s1197797; Roelof van Zwol s1240811
Changed in this revision
| begintotaalscript.cpp | Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r d4f355d72f66 -r 5380d17310e8 begintotaalscript.cpp
--- a/begintotaalscript.cpp Wed Nov 06 16:37:36 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,500 +0,0 @@
-/* - - - - - - - - - - - - - - - - - - - - - - */
-/* */
-/* BRONCODE TEKENROBOT GROEP2: THE DRAWESOME */
-/* ----------------------------------------- */
-/* Belinda Brandwacht s1226290 */
-/* Esther Keulers s1255444 */
-/* Maaike Schotman s1274104 */
-/* Gerjan Wolterink s1197797 */
-/* Roelof van Zwol s1240811 */
-/* */
-/* - - - - - - - - - - - - - - - - - - - - - - */
-
-//libraries aanroepen.
-#include "mbed.h"
-#include "MODSERIAL.h"
-#include "encoder.h"
-
-// Looptimerflag instellen
-
-volatile bool looptimerflag;
-
-void setlooptimerflag(void)
-{
- looptimerflag=true;
-}
-
-
-int main()
-{
- //Communicatie met de pc
- MODSERIAL pc(USBTX,USBRX,64,1024);
- pc.baud(115200); //zet de baud in realterm ook op dit nummer.
-
- //Benoemen inputs van de drie rode knoppen op de robot
- DigitalIn knop1(PTC2);
- DigitalIn knop2(PTB3);
- DigitalIn knop3(PTB2);
-
- //Input mode van de knoppen instellen.
- knop1.mode(PullUp);
- knop2.mode(PullUp);
- knop3.mode(PullUp);
-
- AnalogIn emg1(PTB0); //Analog input emg1
- AnalogIn emg2(PTB1); //Analog input emg2
-
- //Benoemen pinnen waarop de encoder van de motoren is aangeloten.
- Encoder motor1(PTD0,PTC9);
- Encoder motor2(PTD2,PTC8);
-
- /* PWM control to motor */
- PwmOut pwm_motor1(PTA12);
- PwmOut pwm_motor2(PTA5);
-
- /* Direction pin */
- DigitalOut motor1dir(PTD3);
- DigitalOut motor2dir(PTD1);
-
-
- //Variabelen voor menu
- int state = 1;
-
- // Met bool wordt gezegd dat variablen 'true' of 'false' kunnen zijn.
- bool calibratie_rust = false;
- bool calibratie_max = false;
- bool calibratie_motor = false;
- bool meting = false;
-
- //Variabelen EMG/motoren
- double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
- double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
-
- // Variabelen benoemen voor regelaar motor.
- double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm,pi;
- double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, dri, dri_1, utot_r, inputsinus;
- double motor1_maxu, motor2_maxu;
-
-
- pi=3.14159265359;
-
- e1= 0.855930601814815;
- e2= -1.711861203629630;
- e3= 0.855930601814815;
- f1=-1.690996376887443;
- f2= 0.732726030371816;
-
- g1= 0.098259168204717*pow(10.0,-4.0);
- g2= 0.196518336409435*pow(10.0,-4.0);
- g3=0.098259168204717*pow(10.0,-4.0);
- h1= -1.991114292201654;
- h2=0.991153595868935;
-
- be1=0.914969144113083;
- be2= -1.829938288226165;
- be3= 0.914969144113083;
- bf1=-1.822694925196308;
- bf2=0.837181651256022;
-
- bg1= 0.246193004641015*pow(10.0,-5.0);
- bg2= 0.492386009282031 *pow(10.0,-5.0);
- bg3= 0.246193004641015*pow(10.0,-5.0);
- bh1= -1.995557124345789;
- bh2= 0.995566972065975;
-
-
- y1 = 0; /*Initialize y(n-1) = 0*/
- y2 = 0; /*Initialize y(n-2) = 0*/
- z1 = 0; /*Initialize z(n-1) = 0*/
- z2 = 0; /*Initialize z(n-2) = 0*/
- zabs1 = 0; /*Initialize zabs(n-1) = 0*/
- zabs2 = 0; /*Initialize zabs(n-2) = 0*/
- w1 = 0; /*Initialize w(n-1) = 0*/
- w2 = 0; /*Initialize w(n-2) = 0*/
-
- by1 = 0; /*Initialize by(n-1) = 0*/
- by2 = 0; /*Initialize by(n-2) = 0*/
- bz1 = 0; /*Initialize bz(n-1) = 0*/
- bz2 = 0; /*Initialize bz(n-2) = 0*/
- bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
- bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
- bw1 = 0; /*Initialize bw(n-1) = 0*/
- bw2 = 0; /*Initialize bw(n-2) = 0*/
- const float Ts = 0.001; /*Sample Time, const(ant)*/
-
-//Constanten voor tekening: EMGdeel
- xbegin= 0; //beginpositie
- ybegin=0; //beginpositie
- qbegin=5.50; //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
- MaxsnelheidV=150.0; //in mm/s
- MaxsnelheidD=0.5*pi; //in rad/s eerst 0.26 rad/s (15 deg/sec)
-
-//constanten regelaar
- kp_r = 0.006;
- ki_r = 0.005;
-
- Ticker looptimer;
- looptimer.attach(setlooptimerflag,Ts);
-
- while (true) { //oneindige while loop
-
- //Loop die allen gestart wordt als de looptimer is verlopen.
- while(looptimerflag != true);
- looptimerflag = false;
-
-
-// state 1 RUST
- if (state == 1) {
-
- pc.printf("state 1 RUST | knop1 = cal_motor knop2 = MENU EMG knop3 = Tekenen \n\r");
-
-
- if (knop1 == false ) { //als knop 1 is ingedrukt
- state=2;
- wait(0.05); //wacht goed contact knopje
-
- while(knop1 == false) {} //wacht tot knop is losgelaten
- wait(0.05); // wacht goed contact knopje
-
- }
-
- if (knop2 == false ) { //als knop 2 wordt ingedrukt: state is 3
- state=3;
- wait(0.05);
-
- while(knop2 == false) {}
- wait(0.05);
- }
- if (knop3 == false && calibratie_rust == true && calibratie_max == true && calibratie_motor == true) {
- state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn.
- wait(0.05);
-
- while(knop3 == false) {}
- wait(0.05);
- meting=true;
-
- }
- if (knop3 == false && state !=6) {
- wait(0.05);
- pc.printf("Niet alle calibraties uitgevoerd! \n");
- wait(1);
- }
-
-
- }
-
-
-//state2 CAL MOTOR
- if (state == 2) {
- pc.printf("state 2 cal_motor | knop1 = terug naar rust \n\r");
-
- //Als de robot arm met pen met de hand op de begin posisie gezet is moet knop 1 weer worden ingedrukt.
- if (knop1 == false ) {
- state=1;
- wait(0.05);
-
- while(knop1 == false) {}
- wait(0.05);
-
- motor1.setPosition(200.0); //Zeg dat de motor die de arm verplaatst 200 counts heeft, dit komt overeen met (200/CPR)/gearatio = (200/CPR)/50=0.125 dat komt overeen met een hoek van .25 pi
- motor2.setPosition(597.15); //
-
- x=0;
- y=0;
-
- dri_1=0;
-
- calibratie_motor = true;
-
-
-
-
- }
- }
-// state 3 MENU EMG
- if (state == 3) {
- pc.printf("state 3 MENU EMG | knop1= cal EMG rust knop2= rust knop3= cal EMG max \n\r");
-
-
- if (knop2 == false ) { // terug naar state 1
- state=1;
- wait(0.05);
-
- while(knop2 == false) {}
- wait(0.05);
- }
-
- if (knop1 == false ) { // naar state 4
- state=4;
- wait(0.05);
-
- while(knop1 == false) {}
- wait(0.05);
-
- // begingrenswaarden stellen:
- GrenswaardeV=0;
- GrenswaardeD=0;
- meting=true;
- }
-
- if (knop3 == false ) { // naar state 5
- state=5;
- wait(0.05);
-
- while(knop3 == false) {}
- wait(0.05);
-
- // begingrenswaarden stellen:
- MaxwaardeV=0;
- MaxwaardeD=0;
- meting=true;
-
- }
-
-
- }
-
-
- //METEN VAN EMG
- if (meting == true) {
- // pc.printf("TEST CODE: Meting in gang\n");
- yy = emg1.read(); //<measurement function, read from pin>
- byy= emg2.read();
-
- z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
- bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/
-
- y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
- y1= yy; /*y(n) will be y(n-1) on next loop*/
- z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
- z1 = z; /*z(n) will be z(n-1) on next loop*/
-
- by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
- by1= byy; /*y(n) will be y(n-1) on next loop*/
- bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
- bz1 = bz; /*z(n) will be z(n-1) on next loop*/
-
- zabs=abs(z);/*rectify*/
- bzabs=abs(bz);/*rectify*/
-
- w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
- bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/
-
- zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
- zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
- w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
- w1 = w; /*w(n) will be w(n-1) on next loop*/
-
- bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
- bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
- bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
- bw1 = bw; /*w(n) will be w(n-1) on next loop*/
-
- //pc.printf("%f,%f\n",w,bw);
-
- wait(Ts); /*When finished wait (maybe a bit shorter?)*/
-
- if (w>1.0) {
- w=1.0;
- }
- if (bw>1.0) {
- bw=1.0;
- }
-
- }
-
-
-
-// state 4 EMG CAL IN RUST
- if (state == 4) {
- pc.printf("state 4 cal EMG RUST | knop1= MEMNU EMG \n\r");
- pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
- // Nieuwe grenswaarden bepalen
- if (w > GrenswaardeV) {
- GrenswaardeV = w;
- }
- if (bw > GrenswaardeD) {
- GrenswaardeD = bw;
- }
- if (knop1 == false ) { // terug naar state 1
- state=3;
- wait(0.05);
-
- while(knop1 == false) {}
- wait(0.05);
- calibratie_rust = true;
- meting=false;
-
- }
- }
-// state 5 EMG CAL MAX
- if (state == 5) {
- pc.printf("state 5 cal EMG MAX | knop3= EMG MENU \n\r");
- pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
- // Nieuwe maxwaarden bepalen
- if (w > MaxwaardeV) {
- MaxwaardeV = w;
- }
- if (bw > MaxwaardeD) {
- MaxwaardeD = bw;
- }
-
- // einde maxwaarde bepalen:
- if (knop3 == false ) { // terug naar state 3
- state=3;
- wait(0.05);
-
- while(knop3 == false) {}
- wait(0.05);
- calibratie_max = true;
- meting=false;
-
- }
-
- }
-
-// state 6 TEKENEN
-
- if (state == 6) {
- // pc.printf("state 6 tekenen | knop3= rust \n\r");
-
- // EMG
- BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
- BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
-
- SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
- SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
-
- if (SnelheidV<0) {
- SnelheidV=0;
- }
- if (SnelheidD<0) {
- SnelheidD=0;
- }
- if (SnelheidV>MaxsnelheidV) {
- SnelheidV=MaxsnelheidV;
- }
- if (SnelheidD>MaxsnelheidV) {
- SnelheidD=MaxsnelheidV;
- }
-
- deltas = SnelheidV*Ts; //afstand tussen twee meetpunten
- deltaq = SnelheidD*Ts; //verandering hoek tov. de rechtdoor
- q = deltaq+qbegin; // hoekverandering tov. het assenstelsel
-
- deltay = deltas*cos(q); //veranding van pen in de y-richting
- deltax = deltas*sin(q); // verandering van pen in de x-richting
-
- x = xbegin+deltax; // nieuwe postie pen, x-coördinaat
- y = ybegin+deltay; // nieuwe positie pen, y-coöridnaat
-
- if (x>297) { //grenzen A4
- x=297;
- }
- if (y>210) {
- y=210;
- }
-
- if (x<0) { //grenzen A4
- x=0;
- }
- if (y<0) {
- y=0;
- }
-
- xbegin=x; // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
- ybegin=y; // y zal ybgin worden in de volgende loop oftewel:(y(n) will be y(n-1) on next loop)
- qbegin=q; // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
-
- pc.printf("bV=%f, bD=%f, sV=%f, sD=%f, x=%f, y=%f, hoek=%f\n",BereikV,BereikD,SnelheidV,SnelheidD,x,y,(q*57.3));
-
-
-// motoren
-
- if (y>210.0) {
- y=210.0;
- }
- if(y<=0.0) {
- y=0.0;
- }
- if(x>297.0) {
- x=297.0;
- }
- if(x<=0.0) {
- x=0.0;
- }
-
- // Omzetten hoek en straal
- //theta = atan(y/x)+0.25*pi;
- //r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
-
- theta = atan((x+69.80)/(y+69.80));
- r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );
-
-
- theta_pen = motor1.getPosition() * ((.5*pi)/400.0);
- r_pen = motor2.getPosition() * (461.34/2790.13);
-
-
- dtheta = (theta - theta_pen);
- dr = (r - r_pen);
-
- //REGELAAR
- up_r = kp_r * dr; //P-actie
- dri = dri_1 + dr*Ts;
- ui_r = dri_1 * ki_r; //I-actie
- utot_r = up_r + ui_r; //
-
- dri_1 = dri; //nieuwe waardes oud maken.
-
- theta_pwm = (dtheta)*3.0;
- r_pwm = (utot_r/1.0);
-
- //NAAR MOTOR
-
- //Zorgen dat pwm tussen -1 en 1 blijft.
- if(theta_pwm > 1) {
- theta_pwm=1;
- }
- if(theta_pwm < -1) {
- theta_pwm=-1;
- }
- if(r_pwm > 1) {
- r_pwm=1;
- }
- if(r_pwm < -1) {
- r_pwm=-1;
- }
-
- // Bepaal richting waarin motoren moeten draaien
- if(theta_pwm > 0)
- motor1dir.write(1);
- else
- motor1dir.write(0);
- if(r_pwm > 0)
- motor2dir.write(1);
- else
- motor2dir.write(0);
-
- // print naar pc
- //pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus);
-
- //schrijf PWM naar motor
- pwm_motor1.write(abs(theta_pwm));
- pwm_motor2.write(abs(r_pwm));
-
- // terug naar state 1 knop
- if (knop3 == false ) { // terug naar state 1
- state=1;
- meting=false;
- wait(0.05);
-
- while(knop3 == false) {}
- wait(0.05);
- }
-
- }
-
- } //sluiten oneindige while loop
-
-} //afsluiten main
\ No newline at end of file
diff -r d4f355d72f66 -r 5380d17310e8 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 06 17:02:58 2013 +0000
@@ -0,0 +1,496 @@
+/* - - - - - - - - - - - - - - - - - - - - - - */
+/* */
+/* BRONCODE TEKENROBOT GROEP2: THE DRAWESOME */
+/* ----------------------------------------- */
+/* Belinda Brandwacht s1226290 */
+/* Esther Keulers s1255444 */
+/* Maaike Schotman s1274104 */
+/* Gerjan Wolterink s1197797 */
+/* Roelof van Zwol s1240811 */
+/* */
+/* - - - - - - - - - - - - - - - - - - - - - - */
+
+//libraries aanroepen.
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
+
+// Looptimerflag instellen
+
+volatile bool looptimerflag;
+
+void setlooptimerflag(void)
+{
+ looptimerflag=true;
+}
+
+
+int main()
+{
+ //Communicatie met de pc
+ MODSERIAL pc(USBTX,USBRX,64,1024);
+ pc.baud(115200); //zet de baud in realterm ook op dit nummer.
+
+ //Benoemen inputs van de drie rode knoppen op de robot
+ DigitalIn knop1(PTC2);
+ DigitalIn knop2(PTB3);
+ DigitalIn knop3(PTB2);
+
+ //Input mode van de knoppen instellen.
+ knop1.mode(PullUp);
+ knop2.mode(PullUp);
+ knop3.mode(PullUp);
+
+ AnalogIn emg1(PTB0); //Analog input emg1
+ AnalogIn emg2(PTB1); //Analog input emg2
+
+ //Benoemen pinnen waarop de encoder van de motoren is aangeloten.
+ Encoder motor1(PTD0,PTC9);
+ Encoder motor2(PTD2,PTC8);
+
+ /* PWM control to motor */
+ PwmOut pwm_motor1(PTA12);
+ PwmOut pwm_motor2(PTA5);
+
+ /* Direction pin */
+ DigitalOut motor1dir(PTD3);
+ DigitalOut motor2dir(PTD1);
+
+
+ //Variabelen voor menu
+ int state = 1;
+
+ // Met bool wordt gezegd dat variablen 'true' of 'false' kunnen zijn.
+ bool calibratie_rust = false;
+ bool calibratie_max = false;
+ bool calibratie_motor = false;
+ bool meting = false;
+
+ //Variabelen EMG/motoren
+ double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
+ double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
+
+ // Variabelen benoemen voor regelaar motor.
+ double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm,pi;
+ double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, dri, dri_1, utot_r, inputsinus;
+ double motor1_maxu, motor2_maxu;
+
+
+ pi=3.14159265359;
+
+ e1= 0.855930601814815;
+ e2= -1.711861203629630;
+ e3= 0.855930601814815;
+ f1=-1.690996376887443;
+ f2= 0.732726030371816;
+
+ g1= 0.098259168204717*pow(10.0,-4.0);
+ g2= 0.196518336409435*pow(10.0,-4.0);
+ g3=0.098259168204717*pow(10.0,-4.0);
+ h1= -1.991114292201654;
+ h2=0.991153595868935;
+
+ be1=0.914969144113083;
+ be2= -1.829938288226165;
+ be3= 0.914969144113083;
+ bf1=-1.822694925196308;
+ bf2=0.837181651256022;
+
+ bg1= 0.246193004641015*pow(10.0,-5.0);
+ bg2= 0.492386009282031 *pow(10.0,-5.0);
+ bg3= 0.246193004641015*pow(10.0,-5.0);
+ bh1= -1.995557124345789;
+ bh2= 0.995566972065975;
+
+
+ y1 = 0; /*Initialize y(n-1) = 0*/
+ y2 = 0; /*Initialize y(n-2) = 0*/
+ z1 = 0; /*Initialize z(n-1) = 0*/
+ z2 = 0; /*Initialize z(n-2) = 0*/
+ zabs1 = 0; /*Initialize zabs(n-1) = 0*/
+ zabs2 = 0; /*Initialize zabs(n-2) = 0*/
+ w1 = 0; /*Initialize w(n-1) = 0*/
+ w2 = 0; /*Initialize w(n-2) = 0*/
+
+ by1 = 0; /*Initialize by(n-1) = 0*/
+ by2 = 0; /*Initialize by(n-2) = 0*/
+ bz1 = 0; /*Initialize bz(n-1) = 0*/
+ bz2 = 0; /*Initialize bz(n-2) = 0*/
+ bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
+ bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
+ bw1 = 0; /*Initialize bw(n-1) = 0*/
+ bw2 = 0; /*Initialize bw(n-2) = 0*/
+ const float Ts = 0.001; /*Sample Time, const(ant)*/
+
+//Constanten voor tekening: EMGdeel
+ xbegin= 0; //beginpositie
+ ybegin=0; //beginpositie
+ qbegin=5.50; //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
+ MaxsnelheidV=150.0; //in mm/s
+ MaxsnelheidD=0.5*pi; //in rad/s eerst 0.26 rad/s (15 deg/sec)
+
+//constanten regelaar
+ kp_r = 0.006;
+ ki_r = 0.005;
+
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,Ts);
+
+ while (true) { //oneindige while loop
+
+ //Loop die allen gestart wordt als de looptimer is verlopen.
+ while(looptimerflag != true);
+ looptimerflag = false;
+
+
+// state 1 RUST
+ if (state == 1) {
+
+ pc.printf("state 1 RUST | knop1 = cal_motor knop2 = MENU EMG knop3 = Tekenen \n\r");
+
+
+ if (knop1 == false ) { //als knop 1 is ingedrukt
+ state=2;
+ wait(0.05); //wacht goed contact knopje
+
+ while(knop1 == false) {} //wacht tot knop is losgelaten
+ wait(0.05); // wacht goed contact knopje
+
+ }
+
+ if (knop2 == false ) { //als knop 2 wordt ingedrukt: state is 3
+ state=3;
+ wait(0.05);
+
+ while(knop2 == false) {}
+ wait(0.05);
+ }
+ if (knop3 == false && calibratie_rust == true && calibratie_max == true && calibratie_motor == true) {
+ state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn.
+ wait(0.05);
+
+ while(knop3 == false) {}
+ wait(0.05);
+ meting=true;
+
+ }
+ if (knop3 == false && state !=6) {
+ wait(0.05);
+ pc.printf("Niet alle calibraties uitgevoerd! \n");
+ wait(1);
+ }
+
+
+ }
+
+
+//state2 CAL MOTOR
+ if (state == 2) {
+ pc.printf("state 2 cal_motor | knop1 = terug naar rust \n\r");
+
+ //Als de robot arm met pen met de hand op de begin posisie gezet is moet knop 1 weer worden ingedrukt.
+ if (knop1 == false ) {
+ state=1;
+ wait(0.05);
+
+ while(knop1 == false) {}
+ wait(0.05);
+
+ motor1.setPosition(200.0); //Zeg dat de motor die de arm verplaatst 200 counts heeft, dit komt overeen met (200/CPR)/gearatio = (200/CPR)/50=0.125 dat komt overeen met een hoek van pi/4
+ motor2.setPosition(597.15); //Zeg dat de motor die de loper verplaatst 597.15 counts heeft, dit komt overeen met r=98,71 mm
+
+ x=0; //Zeg dat de pen op x=0 staat
+ y=0; //Zeg dat de pen op y=0 staat
+
+ dri_1=0; //vorige integraal van de fout, 0 voor de eerste keer.
+
+ calibratie_motor = true; //zeg dat de calibratie van de motor is uitgevoerd.
+
+
+ }
+ }
+// state 3 MENU EMG
+ if (state == 3) {
+ pc.printf("state 3 MENU EMG | knop1= cal EMG rust knop2= rust knop3= cal EMG max \n\r");
+
+
+ if (knop2 == false ) { // terug naar state 1
+ state=1;
+ wait(0.05);
+
+ while(knop2 == false) {}
+ wait(0.05);
+ }
+
+ if (knop1 == false ) { // naar state 4
+ state=4;
+ wait(0.05);
+
+ while(knop1 == false) {}
+ wait(0.05);
+
+ // begingrenswaarden stellen:
+ GrenswaardeV=0;
+ GrenswaardeD=0;
+ meting=true; //Zet de meting 'aan'
+ }
+
+ if (knop3 == false ) { // naar state 5
+ state=5;
+ wait(0.05);
+
+ while(knop3 == false) {}
+ wait(0.05);
+
+ // begingrenswaarden stellen:
+ MaxwaardeV=0;
+ MaxwaardeD=0;
+ meting=true;
+
+ }
+
+
+ }
+
+
+ //METEN VAN EMG
+ if (meting == true) {
+ // pc.printf("TEST CODE: Meting in gang\n");
+ yy = emg1.read(); //<measurement function, read from pin>
+ byy= emg2.read();
+
+ z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
+ bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/
+
+ y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
+ y1= yy; /*y(n) will be y(n-1) on next loop*/
+ z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
+ z1 = z; /*z(n) will be z(n-1) on next loop*/
+
+ by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
+ by1= byy; /*y(n) will be y(n-1) on next loop*/
+ bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
+ bz1 = bz; /*z(n) will be z(n-1) on next loop*/
+
+ zabs=abs(z);/*rectify*/
+ bzabs=abs(bz);/*rectify*/
+
+ w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
+ bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/
+
+ zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
+ zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
+ w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
+ w1 = w; /*w(n) will be w(n-1) on next loop*/
+
+ bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
+ bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
+ bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
+ bw1 = bw; /*w(n) will be w(n-1) on next loop*/
+
+ //pc.printf("%f,%f\n",w,bw);
+
+
+ if (w>1.0) {
+ w=1.0;
+ }
+ if (bw>1.0) {
+ bw=1.0;
+ }
+
+ }
+
+
+
+// state 4 EMG CAL IN RUST
+ if (state == 4) {
+ pc.printf("state 4 cal EMG RUST | knop1= MEMNU EMG \n\r");
+ pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
+
+ // Nieuwe grenswaarden bepalen
+ if (w > GrenswaardeV) {
+ GrenswaardeV = w;
+ }
+ if (bw > GrenswaardeD) {
+ GrenswaardeD = bw;
+ }
+ if (knop1 == false ) { // terug naar state 1
+ state=3;
+ wait(0.05);
+
+ while(knop1 == false) {}
+ wait(0.05);
+ calibratie_rust = true; //Zeg dat de calibratie van de emg in rust in uitgevoerd.
+ meting=false; //Zet de meting weer 'uit'
+
+ }
+ }
+// state 5 EMG CAL MAX
+ if (state == 5) {
+ pc.printf("state 5 cal EMG MAX | knop3= EMG MENU \n\r");
+ pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
+ // Nieuwe maxwaarden bepalen
+ if (w > MaxwaardeV) {
+ MaxwaardeV = w;
+ }
+ if (bw > MaxwaardeD) {
+ MaxwaardeD = bw;
+ }
+
+ // einde maxwaarde bepalen:
+ if (knop3 == false ) { // terug naar state 3
+ state=3;
+ wait(0.05);
+
+ while(knop3 == false) {}
+ wait(0.05);
+ calibratie_max = true;
+ meting=false;
+
+ }
+
+ }
+
+// state 6 TEKENEN
+
+ if (state == 6) {
+ // pc.printf("state 6 tekenen | knop3= rust \n\r");
+
+ // EMG
+ BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
+ BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
+
+ SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
+ SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
+
+ if (SnelheidV<0) {
+ SnelheidV=0;
+ }
+ if (SnelheidD<0) {
+ SnelheidD=0;
+ }
+ if (SnelheidV>MaxsnelheidV) {
+ SnelheidV=MaxsnelheidV;
+ }
+ if (SnelheidD>MaxsnelheidV) {
+ SnelheidD=MaxsnelheidV;
+ }
+
+ deltas = SnelheidV*Ts; //afstand tussen twee meetpunten
+ deltaq = SnelheidD*Ts; //verandering hoek tov. de rechtdoor
+ q = deltaq+qbegin; // hoekverandering tov. het assenstelsel
+
+ deltay = deltas*cos(q); //veranding van pen in de y-richting
+ deltax = deltas*sin(q); // verandering van pen in de x-richting
+
+ x = xbegin+deltax; // nieuwe postie pen, x-coördinaat
+ y = ybegin+deltay; // nieuwe positie pen, y-coöridnaat
+
+ if (x>297) { //grenzen A4
+ x=297;
+ }
+ if (y>210) {
+ y=210;
+ }
+
+ if (x<0) { //grenzen A4
+ x=0;
+ }
+ if (y<0) {
+ y=0;
+ }
+
+ xbegin=x; // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
+ ybegin=y; // y zal ybgin worden in de volgende loop oftewel:(y(n) will be y(n-1) on next loop)
+ qbegin=q; // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
+
+ pc.printf("bV=%f, bD=%f, sV=%f, sD=%f, x=%f, y=%f, hoek=%f\n",BereikV,BereikD,SnelheidV,SnelheidD,x,y,(q*57.3));
+
+
+// motoren
+
+ if (y>210.0) {
+ y=210.0;
+ }
+ if(y<=0.0) {
+ y=0.0;
+ }
+ if(x>297.0) {
+ x=297.0;
+ }
+ if(x<=0.0) {
+ x=0.0;
+ }
+
+
+ //x en y omzetten in hoek en straal.
+ theta = atan((x+69.80)/(y+69.80));
+ r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );
+
+ //lees de hoek en de straal af uit de motoren.
+ theta_pen = motor1.getPosition() * ((.5*pi)/400.0); //Zet de CPR om in een hoek.
+ r_pen = motor2.getPosition() * (461.34/2790.13); //zet de CPRM om in een straaal van de pen.
+
+ // Berekende het verschil in gewensete en werkelijk hoek en straal.
+ dtheta = (theta - theta_pen);
+ dr = (r - r_pen);
+
+ //REGELAAR
+ up_r = kp_r * dr; //P-actie
+ dri = dri_1 + dr*Ts;
+ ui_r = dri_1 * ki_r; //I-actie
+ utot_r = up_r + ui_r; //
+
+ dri_1 = dri; //nieuwe waardes oud maken.
+
+ theta_pwm = (dtheta)*3.0;
+ r_pwm = (utot_r/1.0);
+
+ //NAAR MOTOR
+
+ //Zorgen dat pwm tussen -1 en 1 blijft.
+ if(theta_pwm > 1) {
+ theta_pwm=1;
+ }
+ if(theta_pwm < -1) {
+ theta_pwm=-1;
+ }
+ if(r_pwm > 1) {
+ r_pwm=1;
+ }
+ if(r_pwm < -1) {
+ r_pwm=-1;
+ }
+
+ // Bepaal richting waarin motoren moeten draaien
+ if(theta_pwm > 0)
+ motor1dir.write(1);
+ else
+ motor1dir.write(0);
+ if(r_pwm > 0)
+ motor2dir.write(1);
+ else
+ motor2dir.write(0);
+
+ // Testcode - print naar pc
+ //pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus);
+
+ //schrijf PWM naar motor
+ pwm_motor1.write(abs(theta_pwm));
+ pwm_motor2.write(abs(r_pwm));
+
+ // terug naar state 1 knop
+ if (knop3 == false ) { // terug naar state 1
+ state=1;
+ meting=false;
+ wait(0.05);
+
+ while(knop3 == false) {}
+ wait(0.05);
+ }
+
+ }
+
+ } //sluiten oneindige while loop
+
+} //afsluiten main
\ No newline at end of file