Projectgroep 20 Biorobotics / Mbed 2 deprecated rki_psuedoinverse

sexy script

Dependencies:   MODSERIAL mbed

main.cpp@0:822bb0d30c21, 2017-11-02 (annotated)

Committer:
Miriam
Date:
Thu Nov 02 23:25:01 2017 +0000
Revision:
0:822bb0d30c21
Child:
1:f7c0b8de5cbb
pseudoinversekinimatica ongetest bitch

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 0:822bb0d30c21 16 double q1=0;
Miriam 0:822bb0d30c21 17 double q2=0;
Miriam 0:822bb0d30c21 18 double T=0.01;
Miriam 0:822bb0d30c21 19 DigitalIn Knopje1 (PTA4); //tijdelijk
Miriam 0:822bb0d30c21 20 DigitalIn Knopje2 (PTC6); //tijdelijk
Miriam 0:822bb0d30c21 21
Miriam 0:822bb0d30c21 22
Miriam 0:822bb0d30c21 23 void IKHAATROBOTS ()
Miriam 0:822bb0d30c21 24 {
Miriam 0:822bb0d30c21 25 bool knop1 = Knopje1;
Miriam 0:822bb0d30c21 26 bool knop2 = Knopje2;
Miriam 0:822bb0d30c21 27
Miriam 0:822bb0d30c21 28 if (knop1 == false){
Miriam 0:822bb0d30c21 29 vx += 0.01;}
Miriam 0:822bb0d30c21 30 else if (knop1 == true){
Miriam 0:822bb0d30c21 31 vx = 0;}
Miriam 0:822bb0d30c21 32 else
Miriam 0:822bb0d30c21 33 {}
Miriam 0:822bb0d30c21 34 if (knop2 == false){
Miriam 0:822bb0d30c21 35 vy += 0.01;}
Miriam 0:822bb0d30c21 36 else if (knop2 ==true){
Miriam 0:822bb0d30c21 37 vy = 0;}
Miriam 0:822bb0d30c21 38 else
Miriam 0:822bb0d30c21 39 {}
Miriam 0:822bb0d30c21 40 Ox = -(0.38*cos(q1+q2) + 0.3*cos(q1));
Miriam 0:822bb0d30c21 41 Oy = -0.38*sin(q1+q2) -0.3*sin(q1);
Miriam 0:822bb0d30c21 42 DetJaq = Oy*0.38*cos(q1+q2) + 0.38*sin(q1+q2)*Ox;
Miriam 0:822bb0d30c21 43 Jaqr1k1 = (0.38*cos(q1+q2)*vx)/DetJaq;
Miriam 0:822bb0d30c21 44 Jaqr1k2 = (0.38*sin(q1+q2)*vy)/DetJaq;
Miriam 0:822bb0d30c21 45 Jaqr2k1 = Ox*vx/DetJaq;
Miriam 0:822bb0d30c21 46 Jaqr2k2 = Oy*vy/DetJaq;
Miriam 0:822bb0d30c21 47 q1_dif = Jaqr1k1 + Jaqr1k2;
Miriam 0:822bb0d30c21 48 q2_dif = Jaqr2k1 + Jaqr2k2;
Miriam 0:822bb0d30c21 49 q1 += q1_dif*T;
Miriam 0:822bb0d30c21 50 q2 += q2_dif*T;
Miriam 0:822bb0d30c21 51 }
Miriam 0:822bb0d30c21 52
Miriam 0:822bb0d30c21 53 int main()
Miriam 0:822bb0d30c21 54 {
Miriam 0:822bb0d30c21 55 Treeko.attach(&IKHAATROBOTS, T);
Miriam 0:822bb0d30c21 56 pc.baud(115200);
Miriam 0:822bb0d30c21 57 }