Eva Krolis / Mbed 2 deprecated Kinematics

Good Jacobian and code Not been tested

Dependencies:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Ramon Waninge

main.cpp@24:e166f8119dbb, 2018-11-01 (annotated)

Committer:
EvaKrolis
Date:
Thu Nov 01 08:46:57 2018 +0000
Revision:
24:e166f8119dbb
Parent:
21:73e1cc927968 
Good version with good Jacobian.; Not been tested yet.

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