Projectgroep 20 Biorobotics / Mbed 2 deprecated RKI_gedeelte_joints

Voor nu even het RKI gedeelte... nog een wiskunde library toevoegen

Dependencies:   mbed

main.cpp@2:6d0cf1e0ee0b, 2017-11-01 (annotated)

Committer:
Annelotte
Date:
Wed Nov 01 13:43:52 2017 +0000
Revision:
2:6d0cf1e0ee0b
Parent:
1:629b077bdae3 
RKI met omzetten van hoek naar 0-1 range

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Annelotte 0:404560011fc8 1 #include "mbed.h"
Annelotte 0:404560011fc8 2
Annelotte 1:629b077bdae3 3 double pi = 3.14159265359;
Annelotte 1:629b077bdae3 4 double SetPx = 0.50; //Setpoint position x-coordinate from changePosition (EMG dependent)
Annelotte 1:629b077bdae3 5 double SetPy = 0.04; //Setpoint position y-coordinate from changePosition (EMG dependent)
Annelotte 1:629b077bdae3 6 double q1 = 0; //Reference position q1 from calibration (only the first time)
Annelotte 1:629b077bdae3 7 double q2 = pi/2; //Reference position q2 from calibration (only the first time)
Annelotte 1:629b077bdae3 8 double L1 = 0.30; //Length arm 1
Annelotte 1:629b077bdae3 9 double L2 = 0.38; //Length arm 2
Annelotte 1:629b077bdae3 10 double K = 1; //Spring constant for movement end-joint to setpoint
Annelotte 1:629b077bdae3 11 double B1 = 1; //Friction coefficient for motor 1
Annelotte 1:629b077bdae3 12 double B2 = 1; //Friction coefficient for motot 2
Annelotte 1:629b077bdae3 13 double T = 1/500; //Desired time step
Annelotte 1:629b077bdae3 14 double Motor1Set; //Motor1 angle
Annelotte 1:629b077bdae3 15 double Motor2Set; //Motor2 angle
Annelotte 0:404560011fc8 16
Annelotte 0:404560011fc8 17 void RKI()
Annelotte 0:404560011fc8 18 {
Annelotte 0:404560011fc8 19 q1 = q1 + ((sin(q1)*L1 + sin(q2)*L2)*SetPy - (cos(q1)*L1 + cos(q2)*L2)*SetPx)*(K*T)/B1; //Calculate desired joint 1 position
Annelotte 0:404560011fc8 20 q2 = q2 + ((SetPy - cos(q1)*L1)*sin(q2)*L2 + (sin(q1)*L1 - SetPx)*cos(q2)*L2)*(K*T)/B2; //Calculate desired joint 2 position
Annelotte 0:404560011fc8 21
Annelotte 1:629b077bdae3 22 Motor1Set = q1/pi; //Calculate the desired motor1 angle from the desired joint positions
Annelotte 1:629b077bdae3 23 Motor2Set = (pi-q2-q1)/pi; //Calculate the desired motor2 angle from the desired joint positions
Annelotte 1:629b077bdae3 24 }
Annelotte 1:629b077bdae3 25
Annelotte 1:629b077bdae3 26 int main()
Annelotte 1:629b077bdae3 27 {
Annelotte 1:629b077bdae3 28 RKI();
Annelotte 0:404560011fc8 29 }
Annelotte 0:404560011fc8 30
Annelotte 0:404560011fc8 31
Annelotte 0:404560011fc8 32
Annelotte 0:404560011fc8 33