Projectgroep 20 Biorobotics / Mbed 2 deprecated rki_psuedoinverse

sexy script

Dependencies:   MODSERIAL mbed

main.cpp@2:935a0c78bc68, 2017-11-03 (annotated)

Committer:
Miriam
Date:
Fri Nov 03 00:08:57 2017 +0000
Revision:
2:935a0c78bc68
Parent:
1:f7c0b8de5cbb 
extra kinematica doneeee (zonder positie, maar met snelheidsbepaling)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Miriam 0:822bb0d30c21 1 #include "mbed.h"
Miriam 0:822bb0d30c21 2 #include "MODSERIAL.h"
Miriam 0:822bb0d30c21 3 MODSERIAL pc(USBTX,USBRX);
Miriam 0:822bb0d30c21 4 Ticker Treecko; //We make a awesome ticker for our control system
Miriam 0:822bb0d30c21 5 double Ox;
Miriam 0:822bb0d30c21 6 double Oy;
Miriam 0:822bb0d30c21 7 double DetJaq;
Miriam 0:822bb0d30c21 8 double Jaqr1k1;
Miriam 0:822bb0d30c21 9 double Jaqr1k2;
Miriam 0:822bb0d30c21 10 double Jaqr2k1;
Miriam 0:822bb0d30c21 11 double Jaqr2k2;
Miriam 0:822bb0d30c21 12 double vx;
Miriam 0:822bb0d30c21 13 double vy;
Miriam 0:822bb0d30c21 14 double q1_dif;
Miriam 0:822bb0d30c21 15 double q2_dif;
Miriam 2:935a0c78bc68 16 double q1=1;
Miriam 2:935a0c78bc68 17 double q2=1;
Miriam 0:822bb0d30c21 18 double T=0.01;
Miriam 2:935a0c78bc68 19 double p;
Miriam 2:935a0c78bc68 20 double q1set;
Miriam 2:935a0c78bc68 21 double q2set;
Miriam 0:822bb0d30c21 22 DigitalIn Knopje1 (PTA4); //tijdelijk
Miriam 0:822bb0d30c21 23 DigitalIn Knopje2 (PTC6); //tijdelijk
Miriam 0:822bb0d30c21 24
Miriam 0:822bb0d30c21 25
Miriam 0:822bb0d30c21 26 void IKHAATROBOTS ()
Miriam 0:822bb0d30c21 27 {
Miriam 0:822bb0d30c21 28 bool knop1 = Knopje1;
Miriam 0:822bb0d30c21 29 bool knop2 = Knopje2;
Miriam 0:822bb0d30c21 30
Miriam 0:822bb0d30c21 31 if (knop1 == false){
Miriam 0:822bb0d30c21 32 vx += 0.01;}
Miriam 0:822bb0d30c21 33 else if (knop1 == true){
Miriam 0:822bb0d30c21 34 vx = 0;}
Miriam 0:822bb0d30c21 35 else
Miriam 0:822bb0d30c21 36 {}
Miriam 0:822bb0d30c21 37 if (knop2 == false){
Miriam 0:822bb0d30c21 38 vy += 0.01;}
Miriam 0:822bb0d30c21 39 else if (knop2 ==true){
Miriam 0:822bb0d30c21 40 vy = 0;}
Miriam 0:822bb0d30c21 41 else
Miriam 0:822bb0d30c21 42 {}
Miriam 2:935a0c78bc68 43
Miriam 2:935a0c78bc68 44 p = q1+q2;
Miriam 0:822bb0d30c21 45 Ox = -(0.38*cos(q1+q2) + 0.3*cos(q1));
Miriam 2:935a0c78bc68 46 Oy = -0.38*sin(q1+q2) - 0.3*sin(q1);
Miriam 0:822bb0d30c21 47 DetJaq = Oy*0.38*cos(q1+q2) + 0.38*sin(q1+q2)*Ox;
Miriam 0:822bb0d30c21 48 Jaqr1k1 = (0.38*cos(q1+q2)*vx)/DetJaq;
Miriam 0:822bb0d30c21 49 Jaqr1k2 = (0.38*sin(q1+q2)*vy)/DetJaq;
Miriam 0:822bb0d30c21 50 Jaqr2k1 = Ox*vx/DetJaq;
Miriam 0:822bb0d30c21 51 Jaqr2k2 = Oy*vy/DetJaq;
Miriam 0:822bb0d30c21 52 q1_dif = Jaqr1k1 + Jaqr1k2;
Miriam 0:822bb0d30c21 53 q2_dif = Jaqr2k1 + Jaqr2k2;
Miriam 0:822bb0d30c21 54 q1 += q1_dif*T;
Miriam 0:822bb0d30c21 55 q2 += q2_dif*T;
Miriam 0:822bb0d30c21 56 }
Miriam 0:822bb0d30c21 57
Miriam 0:822bb0d30c21 58 int main()
Miriam 0:822bb0d30c21 59 {
Miriam 1:f7c0b8de5cbb 60 Treecko.attach(&IKHAATROBOTS, T);
Miriam 0:822bb0d30c21 61 pc.baud(115200);
Miriam 2:935a0c78bc68 62 while (true){
Miriam 2:935a0c78bc68 63 pc.printf("vx = %f, vy = %f, q1 = %f, q2 = %f \n\r",vx, vy, q1, q2);}
Miriam 0:822bb0d30c21 64 }