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@20:11fe0aa7f111, 2018-10-31 (annotated)

Committer:
EvaKrolis
Date:
Wed Oct 31 21:00:51 2018 +0000
Revision:
20:11fe0aa7f111
Parent:
16:deb42ce3c3a1
Child:
21:73e1cc927968
Working version, with possible good frequency; But not the right Jacobian

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