Ramon Waninge / Mbed 2 deprecated Bioroboticsmerge

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Eva Krolis

main.cpp@9:930bd825689f, 2018-10-30 (annotated)

Committer:
Ramonwaninge
Date:
Tue Oct 30 13:35:51 2018 +0000
Revision:
9:930bd825689f
Parent:
8:697aa3c94209
Child:
10:2b965defcde5
Kinematics op de oude manier met qi-1, qi en qi+1. werkt een beetje, maar is volgens arvid niet de goede manier

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 0:779fe292e912 1 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 2 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 3 #include <cmath>
Ramonwaninge 7:b59b762c537e 4 #include "MODSERIAL.h"
Ramonwaninge 0:779fe292e912 5 #define PI 3.14159265
Ramonwaninge 0:779fe292e912 6
Ramonwaninge 3:de8d3ca44a3e 7 MODSERIAL pc(USBTX, USBRX); // connecting to pc
Ramonwaninge 3:de8d3ca44a3e 8 DigitalIn button1(SW3);
Ramonwaninge 7:b59b762c537e 9 DigitalOut ledr(LED1);
Ramonwaninge 7:b59b762c537e 10 DigitalOut led2(LED2);
Ramonwaninge 8:697aa3c94209 11 InterruptIn button2(SW2);
Ramonwaninge 8:697aa3c94209 12
Ramonwaninge 9:930bd825689f 13 // nog te verwijderen/ aan te passen
Ramonwaninge 8:697aa3c94209 14 double theta1;
Ramonwaninge 0:779fe292e912 15
Ramonwaninge 0:779fe292e912 16 //Joe dit zijn de inputsignalen
Ramonwaninge 9:930bd825689f 17 double theta10 ;
Ramonwaninge 9:930bd825689f 18 double theta1i;
Ramonwaninge 8:697aa3c94209 19 double theta11;
Ramonwaninge 8:697aa3c94209 20 double theta4 = PI*0.5;
Ramonwaninge 8:697aa3c94209 21 double emg1;
Ramonwaninge 8:697aa3c94209 22 double emg2;
Ramonwaninge 8:697aa3c94209 23 double emg3;
Ramonwaninge 8:697aa3c94209 24 double thetaflip = 0;
Ramonwaninge 8:697aa3c94209 25 double omega1;
Ramonwaninge 8:697aa3c94209 26 double omega4;
Ramonwaninge 8:697aa3c94209 27 double deltat = 0.01;
Ramonwaninge 0:779fe292e912 28 //Joe dit zijn de constantes
Ramonwaninge 8:697aa3c94209 29 double ll = 200.0;
Ramonwaninge 8:697aa3c94209 30 double lu = 170.0;
Ramonwaninge 8:697aa3c94209 31 double lb = 10.0;
Ramonwaninge 8:697aa3c94209 32 double le = 79.0;
Ramonwaninge 8:697aa3c94209 33 double xbase = 340;
Ramonwaninge 2:0a7a3c0c08d3 34
Ramonwaninge 2:0a7a3c0c08d3 35 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 2:0a7a3c0c08d3 36 //First define the position equation of x
Ramonwaninge 5:d78ed3a3e66a 37 double xendsum;
Ramonwaninge 5:d78ed3a3e66a 38 double xendsqrt1;
Ramonwaninge 5:d78ed3a3e66a 39 double xendsqrt2;
Ramonwaninge 5:d78ed3a3e66a 40 double xend;
Ramonwaninge 2:0a7a3c0c08d3 41 //Now define the pos. eq. of y
Ramonwaninge 6:59744dfe8ea7 42 double yendsum;
Ramonwaninge 6:59744dfe8ea7 43 double yendsqrt1;
Ramonwaninge 6:59744dfe8ea7 44 double yendsqrt2;
Ramonwaninge 6:59744dfe8ea7 45 double yend;
Ramonwaninge 2:0a7a3c0c08d3 46
Ramonwaninge 2:0a7a3c0c08d3 47
Ramonwaninge 2:0a7a3c0c08d3 48 //Hier definieren we de functies
Ramonwaninge 1:f63be2020475 49 Ticker emgcheck;
Ramonwaninge 7:b59b762c537e 50 Ticker emgcheck2;
Ramonwaninge 9:930bd825689f 51 Ticker emgcheck3;
Ramonwaninge 1:f63be2020475 52
Ramonwaninge 0:779fe292e912 53
Ramonwaninge 0:779fe292e912 54 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 7:b59b762c537e 55 void forward(){ //dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
Ramonwaninge 9:930bd825689f 56 // hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
Ramonwaninge 9:930bd825689f 57 //maar daar moet eerst inverse kinematics voor gebeuren.
Ramonwaninge 4:49dfbfcd3577 58 if (button1 == 0){ //als emg1==voorbij treshold,
Ramonwaninge 7:b59b762c537e 59 theta1 = PI*(theta1/PI + 0.1); //double theta1-> plus een paar counts (emg*richting)
Ramonwaninge 4:49dfbfcd3577 60 //double theta4-> plus een paar counts (emg*richting)
Ramonwaninge 4:49dfbfcd3577 61 //default = als x = xbase/2... break, okee dit moet hier niet
Ramonwaninge 4:49dfbfcd3577 62 }
Ramonwaninge 4:49dfbfcd3577 63 else {theta1 = theta1;}
Ramonwaninge 4:49dfbfcd3577 64 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 65 xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
Ramonwaninge 4:49dfbfcd3577 66 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 67 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 6:59744dfe8ea7 68
Ramonwaninge 6:59744dfe8ea7 69 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 6:59744dfe8ea7 70 yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4))));
Ramonwaninge 6:59744dfe8ea7 71 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 72 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 3:de8d3ca44a3e 73 }
Ramonwaninge 7:b59b762c537e 74 void demomode(){} //Alleen nodig in de DEMOMODE
Ramonwaninge 6:59744dfe8ea7 75 //als emg2 == voorbij treshold,
Ramonwaninge 6:59744dfe8ea7 76 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 6:59744dfe8ea7 77 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 6:59744dfe8ea7 78 //reken y door
Ramonwaninge 6:59744dfe8ea7 79 //default = als y = default... break
Ramonwaninge 6:59744dfe8ea7 80 //end
Ramonwaninge 7:b59b762c537e 81 void flip(){
Ramonwaninge 9:930bd825689f 82 if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 -> emg3>= treshold
Ramonwaninge 7:b59b762c537e 83 }
Ramonwaninge 7:b59b762c537e 84
Ramonwaninge 7:b59b762c537e 85 void inverse(){
Ramonwaninge 7:b59b762c537e 86 /*
Ramonwaninge 8:697aa3c94209 87 We willen ergens beginnen; toevoeging: als qi=qi-1, EN emg is over treshold, definieerd omega
Ramonwaninge 7:b59b762c537e 88 Dan de functie qi+1 = qi + (jacobian *(qi - qi-1)/deltaT)deltaT
Ramonwaninge 8:697aa3c94209 89 theta10 = theta1 versie i-1
Ramonwaninge 8:697aa3c94209 90 theta1i = theta1 versie i
Ramonwaninge 8:697aa3c94209 91 theta11 = theta1 versie i+1
Ramonwaninge 7:b59b762c537e 92 */
Ramonwaninge 8:697aa3c94209 93
Ramonwaninge 9:930bd825689f 94 if(theta10 == 0) {
Ramonwaninge 9:930bd825689f 95 theta10 = PI*0.5; //initial conditions, oftewel wat moet hij doen, als hij stil staat?
Ramonwaninge 8:697aa3c94209 96 omega1 = 0.01;
Ramonwaninge 8:697aa3c94209 97 theta1i = theta10+omega1*deltat;
Ramonwaninge 8:697aa3c94209 98 }
Ramonwaninge 9:930bd825689f 99 else{
Ramonwaninge 8:697aa3c94209 100 omega1 = (theta1i - theta10)/deltat;
Ramonwaninge 9:930bd825689f 101
Ramonwaninge 8:697aa3c94209 102 theta1i = theta10+omega1*deltat;
Ramonwaninge 9:930bd825689f 103 }
Ramonwaninge 8:697aa3c94209 104 theta11 = theta1i+ (theta1i - theta10)/deltat*deltat;
Ramonwaninge 8:697aa3c94209 105
Ramonwaninge 8:697aa3c94209 106 theta10 = theta1i;
Ramonwaninge 8:697aa3c94209 107 theta1i = theta11;
Ramonwaninge 9:930bd825689f 108
Ramonwaninge 7:b59b762c537e 109 }
Ramonwaninge 0:779fe292e912 110
Ramonwaninge 0:779fe292e912 111 int main()
Ramonwaninge 0:779fe292e912 112 {
Ramonwaninge 2:0a7a3c0c08d3 113
Ramonwaninge 2:0a7a3c0c08d3 114 pc.baud(115200);
Ramonwaninge 0:779fe292e912 115 //default = theta1 = theta4 = pi/2
Ramonwaninge 7:b59b762c537e 116 emgcheck.attach(forward, 0.1);
Ramonwaninge 7:b59b762c537e 117 emgcheck2.attach(flip, 0.1);
Ramonwaninge 9:930bd825689f 118 emgcheck.attach(inverse, 0.1);
Ramonwaninge 8:697aa3c94209 119 ledr=1;
Ramonwaninge 9:930bd825689f 120 pc.printf("%f", theta10);
Ramonwaninge 3:de8d3ca44a3e 121 while(true){
Ramonwaninge 8:697aa3c94209 122
Ramonwaninge 8:697aa3c94209 123 if (button1 == 0){
Ramonwaninge 8:697aa3c94209 124 pc.printf("\n\r %f %f \n\r", theta10,theta1i);
Ramonwaninge 8:697aa3c94209 125 wait(1.5);
Ramonwaninge 3:de8d3ca44a3e 126 }
Ramonwaninge 8:697aa3c94209 127 }
Ramonwaninge 0:779fe292e912 128 }