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@25:1123d100d964, 2018-11-01 (annotated)

Committer:
Ramonwaninge
Date:
Thu Nov 01 10:35:52 2018 +0000
Revision:
25:1123d100d964
Parent:
24:e166f8119dbb
Child:
26:efd04dec7710
Na de aanpassingen van Arvid, werkt nog niet...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 15:38258e6b6e91 1 //Libraries
Ramonwaninge 0:779fe292e912 2 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 3 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 4 #include <cmath>
Ramonwaninge 12:8d3bc1fa2321 5 #include "MODSERIAL.h"
Ramonwaninge 13:f77c5f196161 6 #include "BiQuad.h"
Ramonwaninge 13:f77c5f196161 7 #include <algorithm>
Ramonwaninge 25:1123d100d964 8 //#include "FastPWM.h"
Ramonwaninge 0:779fe292e912 9 #define PI 3.14159265
Ramonwaninge 14:e3fe54f0a4b4 10
Ramonwaninge 14:e3fe54f0a4b4 11 //Inputs and outputs
EvaKrolis 20:11fe0aa7f111 12 MODSERIAL pc(USBTX, USBRX); //Connecting to PC
Ramonwaninge 14:e3fe54f0a4b4 13 AnalogIn EMG0In(A0); //EMG input 0
Ramonwaninge 14:e3fe54f0a4b4 14 AnalogIn EMG1In(A1); //EMG input 1
Ramonwaninge 14:e3fe54f0a4b4 15 InterruptIn button(SW3); //Define button
Ramonwaninge 14:e3fe54f0a4b4 16 DigitalOut greenled(LED_GREEN); //Green led
Ramonwaninge 14:e3fe54f0a4b4 17 DigitalOut blueled(LED_BLUE); //Blue led
Ramonwaninge 14:e3fe54f0a4b4 18 DigitalOut redled(LED_RED); //Red led
Ramonwaninge 13:f77c5f196161 19
Ramonwaninge 15:38258e6b6e91 20 //Constants
Ramonwaninge 25:1123d100d964 21 const float ll = 230; //Length of the lower arm
Ramonwaninge 25:1123d100d964 22 const float lu = 198; //Length of the upper arm
Ramonwaninge 25:1123d100d964 23 const float lb = 50; //Length of the part between the upper arms
Ramonwaninge 25:1123d100d964 24 const float le = 79; //Length of the end-effector beam
Ramonwaninge 25:1123d100d964 25 const float xbase = 450-lb; //Length between the motors
Ramonwaninge 15:38258e6b6e91 26 const int Length = 10000; //Length of the array for the calibration
Ramonwaninge 15:38258e6b6e91 27 const int Parts = 50; //Mean average filter over 50 values
Ramonwaninge 15:38258e6b6e91 28
Ramonwaninge 14:e3fe54f0a4b4 29 //parameters for kinematics
Ramonwaninge 25:1123d100d964 30 float theta1 = PI*0.49; //Angle of the left motor
Ramonwaninge 25:1123d100d964 31 float theta4 = PI*0.49; //Angle of the right motor
Ramonwaninge 25:1123d100d964 32 float thetaflip = 0; //Angle of the flipping motor
Ramonwaninge 25:1123d100d964 33 float prefx; //Desired x velocity
Ramonwaninge 25:1123d100d964 34 float prefy; //Desired y velocity "
Ramonwaninge 25:1123d100d964 35 float deltat = 0.01;
Ramonwaninge 25:1123d100d964 36 float iJ[2][2];
Ramonwaninge 25:1123d100d964 37 //Time step (dependent on sample frequency)
EvaKrolis 20:11fe0aa7f111 38
EvaKrolis 20:11fe0aa7f111 39 //Kinematics parameters for x
Ramonwaninge 25:1123d100d964 40 float xendsum;
Ramonwaninge 25:1123d100d964 41 float xendsqrt1;
Ramonwaninge 25:1123d100d964 42 float xendsqrt2;
Ramonwaninge 25:1123d100d964 43 float xend;
Ramonwaninge 25:1123d100d964 44 float jacobiana;
Ramonwaninge 25:1123d100d964 45 float jacobianc;
EvaKrolis 20:11fe0aa7f111 46
EvaKrolis 20:11fe0aa7f111 47 //Kinematics parameters for y
Ramonwaninge 25:1123d100d964 48 float yendsum;
Ramonwaninge 25:1123d100d964 49 float yendsqrt1;
Ramonwaninge 25:1123d100d964 50 float yendsqrt2;
Ramonwaninge 25:1123d100d964 51 float yend;
Ramonwaninge 25:1123d100d964 52 float jacobianb;
Ramonwaninge 25:1123d100d964 53 float jacobiand;
Ramonwaninge 14:e3fe54f0a4b4 54
Ramonwaninge 14:e3fe54f0a4b4 55 //Parameters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 56 float EMG0; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 57 float EMG0filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 58 float EMG0filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 59 float EMG0Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 60 float Sum0 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 61 float EMG0Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 62 int ReadCal0 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 63 float MaxValue0 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 64 float Threshold0 = 0; //Threshold for the first EMG signal
Ramonwaninge 13:f77c5f196161 65
Ramonwaninge 14:e3fe54f0a4b4 66 //Parameters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 67 float EMG1; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 68 float EMG1filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 69 float EMG1filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 70 float EMG1Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 71 float Sum1 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 72 float EMG1Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 73 int ReadCal1 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 74 float MaxValue1 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 75 float Threshold1 = 0; //Threshold for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 76
Ramonwaninge 14:e3fe54f0a4b4 77 //Filter variables
Ramonwaninge 14:e3fe54f0a4b4 78 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
Ramonwaninge 14:e3fe54f0a4b4 79 BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
Ramonwaninge 14:e3fe54f0a4b4 80 BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
Ramonwaninge 14:e3fe54f0a4b4 81 BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
Ramonwaninge 14:e3fe54f0a4b4 82 BiQuadChain filter0; //Make chain of filters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 83 BiQuadChain filter1; //Make chain of filters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 84
Ramonwaninge 13:f77c5f196161 85 //Timers and Tickers
Ramonwaninge 13:f77c5f196161 86 Ticker kin; //Timer for calculating x,y,theta1,theta4
Ramonwaninge 13:f77c5f196161 87 Ticker simulateval; //Timer that prints the values for x,y, and angles
Ramonwaninge 13:f77c5f196161 88 Ticker ReadUseEMG0_timer; //Timer to read, filter and use the EMG
Ramonwaninge 15:38258e6b6e91 89 Ticker EMGCalibration0_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 90 Ticker FindMax0_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 91 Ticker ReadUseEMG1_timer; //Timer to read, filter and use the EMG
Ramonwaninge 15:38258e6b6e91 92 Ticker EMGCalibration1_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 93 Ticker FindMax1_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 94 Ticker SwitchState_timer; //Timer to switch from the Calibration to the working mode
Ramonwaninge 25:1123d100d964 95 Timer local_timer;
Ramonwaninge 13:f77c5f196161 96
Ramonwaninge 14:e3fe54f0a4b4 97 //Bool for movement
Ramonwaninge 14:e3fe54f0a4b4 98 bool xMove = false; //Bool for the x-movement
Ramonwaninge 14:e3fe54f0a4b4 99 bool yMove = false; //Bool for the y-movement
Ramonwaninge 14:e3fe54f0a4b4 100
Ramonwaninge 14:e3fe54f0a4b4 101 //Parameters for the state machine
Ramonwaninge 14:e3fe54f0a4b4 102 enum States {Calibration, WorkingMode}; //Initialize state machine
EvaKrolis 20:11fe0aa7f111 103 States CurrentState = Calibration; //Start in the calibration mode
Ramonwaninge 14:e3fe54f0a4b4 104 bool StateBool = true; //Bool to first go in a state
Ramonwaninge 14:e3fe54f0a4b4 105 bool SwitchStateBool = false; //Bool to switch from calibration to working mode
Ramonwaninge 14:e3fe54f0a4b4 106
Ramonwaninge 14:e3fe54f0a4b4 107 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 108 void ReadUseEMG0()
Ramonwaninge 15:38258e6b6e91 109 {
Ramonwaninge 15:38258e6b6e91 110 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 14:e3fe54f0a4b4 111 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 14:e3fe54f0a4b4 112 }
Ramonwaninge 15:38258e6b6e91 113
Ramonwaninge 15:38258e6b6e91 114 Sum0 = 0;
Ramonwaninge 15:38258e6b6e91 115 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 116 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 15:38258e6b6e91 117 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 118 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 119
Ramonwaninge 15:38258e6b6e91 120 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 121 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 122 }
Ramonwaninge 15:38258e6b6e91 123 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 124
Ramonwaninge 15:38258e6b6e91 125 if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
Ramonwaninge 25:1123d100d964 126 //redled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 127 xMove = true; //Set movement to true
Ramonwaninge 15:38258e6b6e91 128 } else {
Ramonwaninge 25:1123d100d964 129 //redled = 1; //Otherwise turn the LED off
Ramonwaninge 15:38258e6b6e91 130 xMove = false; //Otherwise set movement to false
Ramonwaninge 15:38258e6b6e91 131 }
Ramonwaninge 15:38258e6b6e91 132 }
Ramonwaninge 15:38258e6b6e91 133 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 134 void ReadUseEMG1()
Ramonwaninge 15:38258e6b6e91 135 {
Ramonwaninge 15:38258e6b6e91 136 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 137 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 138 }
Ramonwaninge 15:38258e6b6e91 139
Ramonwaninge 15:38258e6b6e91 140 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 141 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 142 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 143 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 144 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 145
Ramonwaninge 15:38258e6b6e91 146 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 147 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 148 }
Ramonwaninge 15:38258e6b6e91 149 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 150
Ramonwaninge 15:38258e6b6e91 151 if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
Ramonwaninge 15:38258e6b6e91 152 greenled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 153 yMove = true; //Set y movement to true
Ramonwaninge 15:38258e6b6e91 154 } else {
Ramonwaninge 15:38258e6b6e91 155 greenled = 1; //Otherwise turn the LED off
EvaKrolis 16:deb42ce3c3a1 156 yMove = false; //Otherwise set y movement to false
Ramonwaninge 15:38258e6b6e91 157 }
Ramonwaninge 15:38258e6b6e91 158 }
Ramonwaninge 15:38258e6b6e91 159
Ramonwaninge 15:38258e6b6e91 160
Ramonwaninge 15:38258e6b6e91 161 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 162 void CalibrateEMG0()
Ramonwaninge 15:38258e6b6e91 163 {
Ramonwaninge 15:38258e6b6e91 164 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 165 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 166 }
Ramonwaninge 15:38258e6b6e91 167
Ramonwaninge 14:e3fe54f0a4b4 168 Sum0 = 0;
Ramonwaninge 14:e3fe54f0a4b4 169 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 14:e3fe54f0a4b4 170 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 14:e3fe54f0a4b4 171 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 14:e3fe54f0a4b4 172 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 173
Ramonwaninge 15:38258e6b6e91 174 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 14:e3fe54f0a4b4 175 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 176 }
Ramonwaninge 15:38258e6b6e91 177 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 178
Ramonwaninge 15:38258e6b6e91 179 ReadCal0++;
Ramonwaninge 15:38258e6b6e91 180 }
Ramonwaninge 15:38258e6b6e91 181
Ramonwaninge 15:38258e6b6e91 182 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 183 void CalibrateEMG1()
Ramonwaninge 15:38258e6b6e91 184 {
Ramonwaninge 15:38258e6b6e91 185 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 186 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 187 }
Ramonwaninge 15:38258e6b6e91 188
Ramonwaninge 15:38258e6b6e91 189 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 190 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 191 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 192 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 193 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 194
Ramonwaninge 15:38258e6b6e91 195 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 196 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 197 }
Ramonwaninge 15:38258e6b6e91 198 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 199
Ramonwaninge 15:38258e6b6e91 200 ReadCal1++;
Ramonwaninge 15:38258e6b6e91 201 }
Ramonwaninge 15:38258e6b6e91 202
Ramonwaninge 15:38258e6b6e91 203 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 204 void FindMax0()
Ramonwaninge 15:38258e6b6e91 205 {
Ramonwaninge 15:38258e6b6e91 206 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 207 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 208 pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
Ramonwaninge 15:38258e6b6e91 209 FindMax0_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 210 }
Ramonwaninge 15:38258e6b6e91 211
Ramonwaninge 15:38258e6b6e91 212 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 213 void FindMax1()
Ramonwaninge 15:38258e6b6e91 214 {
Ramonwaninge 15:38258e6b6e91 215 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 216 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 217 pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
Ramonwaninge 15:38258e6b6e91 218 FindMax1_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 219 }
Ramonwaninge 15:38258e6b6e91 220
Ramonwaninge 15:38258e6b6e91 221 //Function to stop the reading of the EMG
Ramonwaninge 15:38258e6b6e91 222 void StopProgram()
Ramonwaninge 15:38258e6b6e91 223 {
Ramonwaninge 15:38258e6b6e91 224 greenled = 1; //Turn the LEDs off
Ramonwaninge 15:38258e6b6e91 225 blueled = 1;
Ramonwaninge 15:38258e6b6e91 226 redled = 1;
Ramonwaninge 15:38258e6b6e91 227 exit (0); //Abort mission!!
Ramonwaninge 15:38258e6b6e91 228 }
Ramonwaninge 15:38258e6b6e91 229
Ramonwaninge 15:38258e6b6e91 230 //Function to switch a state
Ramonwaninge 25:1123d100d964 231 /*void SwitchState()
Ramonwaninge 15:38258e6b6e91 232 {
Ramonwaninge 15:38258e6b6e91 233 SwitchStateBool = true; //Set the bool for the start of a state to true
Ramonwaninge 15:38258e6b6e91 234 SwitchState_timer.detach(); //Use this function once
Ramonwaninge 25:1123d100d964 235 }*/
Ramonwaninge 14:e3fe54f0a4b4 236
Ramonwaninge 14:e3fe54f0a4b4 237
Ramonwaninge 25:1123d100d964 238 void FK(float &xend_, float &yend_, float theta1_, float theta4_)
Ramonwaninge 25:1123d100d964 239 {
Ramonwaninge 25:1123d100d964 240 float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
Ramonwaninge 25:1123d100d964 241 float 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 25:1123d100d964 242 float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
Ramonwaninge 25:1123d100d964 243 xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
Ramonwaninge 25:1123d100d964 244 //hieronder rekenen we yendeffector door;
Ramonwaninge 25:1123d100d964 245 float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
Ramonwaninge 25:1123d100d964 246 float 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 25:1123d100d964 247 float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
Ramonwaninge 25:1123d100d964 248 yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
Ramonwaninge 25:1123d100d964 249 }
Ramonwaninge 14:e3fe54f0a4b4 250
Ramonwaninge 12:8d3bc1fa2321 251 //dit wordt aangeroepen in de tickerfunctie
Ramonwaninge 25:1123d100d964 252 void inverse(float prex, float prey)
Ramonwaninge 15:38258e6b6e91 253 {
Ramonwaninge 12:8d3bc1fa2321 254 /*
Ramonwaninge 12:8d3bc1fa2321 255 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
Ramonwaninge 12:8d3bc1fa2321 256 ofwel
Ramonwaninge 12:8d3bc1fa2321 257 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
Ramonwaninge 12:8d3bc1fa2321 258 waar Pref = emg signaal
Ramonwaninge 12:8d3bc1fa2321 259 */ //achtergrondinfo hierboven...
Ramonwaninge 15:38258e6b6e91 260 //
Ramonwaninge 15:38258e6b6e91 261
Ramonwaninge 25:1123d100d964 262 theta1 += (prefx*(iJ[0][0])+iJ[0][1]*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
Ramonwaninge 25:1123d100d964 263 theta4 += (prefx*iJ[1][0]+iJ[1][1]*prey)*deltat;//" "
Ramonwaninge 15:38258e6b6e91 264 //Hier worden xend en yend doorgerekend, die formules kan je overslaan
Ramonwaninge 25:1123d100d964 265
Ramonwaninge 25:1123d100d964 266 FK(xend,yend,theta1,theta4);
Ramonwaninge 12:8d3bc1fa2321 267
Ramonwaninge 3:de8d3ca44a3e 268 }
Ramonwaninge 25:1123d100d964 269
Ramonwaninge 25:1123d100d964 270
Ramonwaninge 25:1123d100d964 271
Ramonwaninge 12:8d3bc1fa2321 272 //deze onderstaande tickerfunctie wordt aangeroepen
Ramonwaninge 12:8d3bc1fa2321 273 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 274 {
Ramonwaninge 12:8d3bc1fa2321 275
Ramonwaninge 12:8d3bc1fa2321 276 //Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
Ramonwaninge 25:1123d100d964 277 float xend1,xend2,xend3,yend1,yend2,yend3;
Ramonwaninge 25:1123d100d964 278 const float dq = 0.0001;
Ramonwaninge 25:1123d100d964 279 FK(xend1,yend1,theta1,theta4);
Ramonwaninge 25:1123d100d964 280 FK(xend2,yend2,theta1+dq,theta4);
Ramonwaninge 25:1123d100d964 281 FK(xend3,yend3,theta1,theta4+dq);
Ramonwaninge 25:1123d100d964 282
Ramonwaninge 25:1123d100d964 283 float a,b,c,d;
Ramonwaninge 25:1123d100d964 284 a = xend2-xend1;
Ramonwaninge 25:1123d100d964 285 b = xend3-xend1;
Ramonwaninge 25:1123d100d964 286 c = yend2-yend1;
Ramonwaninge 25:1123d100d964 287 d = yend3-yend1;
Ramonwaninge 25:1123d100d964 288
Ramonwaninge 25:1123d100d964 289 float Q = 1/(a*d-b*c)/dq;
Ramonwaninge 25:1123d100d964 290
Ramonwaninge 25:1123d100d964 291
Ramonwaninge 25:1123d100d964 292 iJ[0][0] = d*Q;
Ramonwaninge 25:1123d100d964 293 iJ[0][1]= -c*Q;
Ramonwaninge 25:1123d100d964 294 iJ[1][0] = -b*Q;
Ramonwaninge 25:1123d100d964 295 iJ[1][1] = a*Q;
Ramonwaninge 25:1123d100d964 296 /*
Ramonwaninge 12:8d3bc1fa2321 297
EvaKrolis 24:e166f8119dbb 298 jacobiana = (500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 299 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 24:e166f8119dbb 300 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 301 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.))/
Ramonwaninge 25:1123d100d964 302 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 303 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 24:e166f8119dbb 304 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 305 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 24:e166f8119dbb 306 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 307 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 308 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
Ramonwaninge 25:1123d100d964 309 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 24:e166f8119dbb 310 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 311 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 312 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 24:e166f8119dbb 313 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 24:e166f8119dbb 314 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 315 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 24:e166f8119dbb 316 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 317 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
EvaKrolis 24:e166f8119dbb 318
EvaKrolis 24:e166f8119dbb 319 jacobianb = (-250*(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 320 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 321 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
EvaKrolis 24:e166f8119dbb 322 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))))/
Ramonwaninge 25:1123d100d964 323 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 324 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 24:e166f8119dbb 325 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 326 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 24:e166f8119dbb 327 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 328 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 329 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
Ramonwaninge 25:1123d100d964 330 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 24:e166f8119dbb 331 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 332 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 333 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 24:e166f8119dbb 334 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 24:e166f8119dbb 335 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 336 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 24:e166f8119dbb 337 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 338 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
Ramonwaninge 12:8d3bc1fa2321 339
EvaKrolis 24:e166f8119dbb 340 jacobianc = (-500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 341 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 24:e166f8119dbb 342 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 343 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.))/
Ramonwaninge 25:1123d100d964 344 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 345 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 24:e166f8119dbb 346 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 347 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 24:e166f8119dbb 348 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 349 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 350 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
Ramonwaninge 25:1123d100d964 351 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 24:e166f8119dbb 352 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 353 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 354 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 24:e166f8119dbb 355 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 24:e166f8119dbb 356 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 357 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 24:e166f8119dbb 358 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 359 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
Ramonwaninge 12:8d3bc1fa2321 360
EvaKrolis 24:e166f8119dbb 361 jacobiand = (250*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 362 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 363 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 24:e166f8119dbb 364 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))))/
Ramonwaninge 25:1123d100d964 365 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 366 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 24:e166f8119dbb 367 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 368 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 24:e166f8119dbb 369 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 370 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 371 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
Ramonwaninge 25:1123d100d964 372 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 24:e166f8119dbb 373 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
Ramonwaninge 25:1123d100d964 374 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 24:e166f8119dbb 375 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 24:e166f8119dbb 376 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 24:e166f8119dbb 377 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
Ramonwaninge 25:1123d100d964 378 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 24:e166f8119dbb 379 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 24:e166f8119dbb 380 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
EvaKrolis 20:11fe0aa7f111 381 //vanaf hier weer door met lezen!
Ramonwaninge 25:1123d100d964 382 */
Ramonwaninge 25:1123d100d964 383 prefx = 1*xMove; //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
Ramonwaninge 25:1123d100d964 384 // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
Ramonwaninge 25:1123d100d964 385 prefy = 1*yMove; //sw2,
Ramonwaninge 12:8d3bc1fa2321 386 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 387 }
Ramonwaninge 12:8d3bc1fa2321 388
EvaKrolis 16:deb42ce3c3a1 389 void printvalue()
EvaKrolis 16:deb42ce3c3a1 390 {
Ramonwaninge 25:1123d100d964 391 pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 = %f",xend, yend,theta1,theta4); // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
EvaKrolis 16:deb42ce3c3a1 392
EvaKrolis 16:deb42ce3c3a1 393 }
Ramonwaninge 12:8d3bc1fa2321 394
Ramonwaninge 15:38258e6b6e91 395 //State Machine
Ramonwaninge 15:38258e6b6e91 396 void StateMachine()
Ramonwaninge 15:38258e6b6e91 397 {
Ramonwaninge 25:1123d100d964 398 redled = 1;
Ramonwaninge 15:38258e6b6e91 399 switch(CurrentState) { //Determine in which state you are
Ramonwaninge 15:38258e6b6e91 400
Ramonwaninge 25:1123d100d964 401 case Calibration:
Ramonwaninge 25:1123d100d964 402 //Calibration mode
Ramonwaninge 15:38258e6b6e91 403 if(StateBool) { //If you go into this state
Ramonwaninge 15:38258e6b6e91 404 pc.printf("You can start calibrating. \n\r"); //Print that you are in this state
Ramonwaninge 15:38258e6b6e91 405 StateBool = false; //Set the bool for the start of a state to false
Ramonwaninge 15:38258e6b6e91 406 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 407 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
Ramonwaninge 25:1123d100d964 408 //SwitchState_timer.attach(&SwitchState,20); //Switch to the next state after 16 seconds
Ramonwaninge 25:1123d100d964 409 local_timer.reset();
Ramonwaninge 25:1123d100d964 410 local_timer.start();
Ramonwaninge 15:38258e6b6e91 411 blueled = 0;
Ramonwaninge 15:38258e6b6e91 412 }
Ramonwaninge 15:38258e6b6e91 413
Ramonwaninge 25:1123d100d964 414 CalibrateEMG0();
Ramonwaninge 25:1123d100d964 415 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
Ramonwaninge 25:1123d100d964 416
Ramonwaninge 25:1123d100d964 417 if (local_timer.read() > 20) { //If the bool is changed
Ramonwaninge 15:38258e6b6e91 418 CurrentState = WorkingMode; //Change the state to the working mode
Ramonwaninge 15:38258e6b6e91 419 StateBool = true; //Set the start of a state bool to true
Ramonwaninge 25:1123d100d964 420 //SwitchStateBool = false; //Set the switch bool to false
Ramonwaninge 15:38258e6b6e91 421 }
Ramonwaninge 15:38258e6b6e91 422 break;
Ramonwaninge 15:38258e6b6e91 423
Ramonwaninge 15:38258e6b6e91 424 case WorkingMode: //Mode to get the robot working
Ramonwaninge 15:38258e6b6e91 425 if (StateBool) { //If you start to go in this state
Ramonwaninge 15:38258e6b6e91 426 pc.printf("You are know in the working mode. \r\n"); //Print in which mode you are
Ramonwaninge 15:38258e6b6e91 427 StateBool = false; //Set the start of state bool to true
Ramonwaninge 25:1123d100d964 428 //EMGCalibration0_timer.detach(); //Detach the the calibration
Ramonwaninge 25:1123d100d964 429 //EMGCalibration1_timer.detach(); //Detach the calibration
Ramonwaninge 25:1123d100d964 430 //ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005); //Start the use of EMG
Ramonwaninge 25:1123d100d964 431 //ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005); //Start the use of EMG
Ramonwaninge 25:1123d100d964 432 //kin.attach(kinematics, 0.005); // roep de ticker aan (
Ramonwaninge 25:1123d100d964 433 //simulateval.attach(printvalue, 0.1);
Ramonwaninge 15:38258e6b6e91 434 }
Ramonwaninge 25:1123d100d964 435 blueled = 1;
Ramonwaninge 25:1123d100d964 436
Ramonwaninge 25:1123d100d964 437 ReadUseEMG0();
Ramonwaninge 25:1123d100d964 438 ReadUseEMG1();
Ramonwaninge 25:1123d100d964 439 kinematics();
Ramonwaninge 25:1123d100d964 440
Ramonwaninge 25:1123d100d964 441 //motorcontroller
Ramonwaninge 25:1123d100d964 442 //Set the blue led off
EvaKrolis 16:deb42ce3c3a1 443 //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
Ramonwaninge 15:38258e6b6e91 444 break;
Ramonwaninge 15:38258e6b6e91 445 }
Ramonwaninge 25:1123d100d964 446 redled = 0;
Ramonwaninge 15:38258e6b6e91 447 }
Ramonwaninge 15:38258e6b6e91 448
Ramonwaninge 0:779fe292e912 449 int main()
Ramonwaninge 0:779fe292e912 450 {
Ramonwaninge 25:1123d100d964 451 Ticker hoofdticker;
Ramonwaninge 12:8d3bc1fa2321 452 //Initial conditions
EvaKrolis 21:73e1cc927968 453 theta1 = PI*0.49; //Angle of the left motor
Ramonwaninge 12:8d3bc1fa2321 454 theta4 = PI*0.49;
Ramonwaninge 2:0a7a3c0c08d3 455 pc.baud(115200);
Ramonwaninge 13:f77c5f196161 456 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 457 blueled = 1;
Ramonwaninge 13:f77c5f196161 458 redled = 1;
Ramonwaninge 13:f77c5f196161 459 filter0.add(&Notch50_0).add(&High0); //Make filter chain for the first EMG
Ramonwaninge 13:f77c5f196161 460 filter1.add(&Notch50_1).add(&High1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 461 button.rise(StopProgram); //If the button is pressed, stop program
Ramonwaninge 25:1123d100d964 462 hoofdticker.attach(&StateMachine,0.002);
Ramonwaninge 12:8d3bc1fa2321 463 while(true) {
Ramonwaninge 25:1123d100d964 464 printvalue();
Ramonwaninge 25:1123d100d964 465 wait(0.75);
Ramonwaninge 25:1123d100d964 466 //Start the state machine
Ramonwaninge 8:697aa3c94209 467 }
Ramonwaninge 0:779fe292e912 468 }