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@21:73e1cc927968, 2018-11-01 (annotated)

Committer:
EvaKrolis
Date:
Thu Nov 01 08:42:44 2018 +0000
Revision:
21:73e1cc927968
Parent:
20:11fe0aa7f111
Child:
24:e166f8119dbb
Good version, 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 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 20:11fe0aa7f111 266 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 267 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
EvaKrolis 20:11fe0aa7f111 268 ((-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 269 (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 270 (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 271 sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 272 (-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 273 sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
EvaKrolis 20:11fe0aa7f111 274 (-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 275 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
EvaKrolis 20:11fe0aa7f111 276 ((-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 277 (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 278 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 279 (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 280 ((-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 281 (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
EvaKrolis 20:11fe0aa7f111 282 ((-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 283 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 284 (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 285 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 286
Ramonwaninge 12:8d3bc1fa2321 287 jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
EvaKrolis 20:11fe0aa7f111 288 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 289 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 290 (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 291 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
EvaKrolis 20:11fe0aa7f111 292 (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 293 (-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 294 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
EvaKrolis 20:11fe0aa7f111 295 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 296 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
EvaKrolis 20:11fe0aa7f111 297 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
EvaKrolis 20:11fe0aa7f111 298 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 299 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
EvaKrolis 20:11fe0aa7f111 300 ((-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 301 (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 302 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 303 (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 304 ((-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 305 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
EvaKrolis 20:11fe0aa7f111 306 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
EvaKrolis 20:11fe0aa7f111 307 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 308 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 309 (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 310 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 311
EvaKrolis 20:11fe0aa7f111 312 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 313 (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 314 ((-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 315 (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
EvaKrolis 20:11fe0aa7f111 316 (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 317 sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 318 (-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 319 sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
EvaKrolis 20:11fe0aa7f111 320 (-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 321 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
EvaKrolis 20:11fe0aa7f111 322 ((-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 323 (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 324 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 325 (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 326 ((-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 327 (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
EvaKrolis 20:11fe0aa7f111 328 ((-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 329 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 330 (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 331 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 332
EvaKrolis 20:11fe0aa7f111 333 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 334 sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 335 (-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 336 sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
EvaKrolis 20:11fe0aa7f111 337 (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 338 sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 339 (-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 340 sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
EvaKrolis 20:11fe0aa7f111 341 (-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 342 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
EvaKrolis 20:11fe0aa7f111 343 ((-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 344 (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 345 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 346 (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 347 ((-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 348 (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
EvaKrolis 20:11fe0aa7f111 349 ((-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 350 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
EvaKrolis 20:11fe0aa7f111 351 (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 352 sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
EvaKrolis 20:11fe0aa7f111 353 //vanaf hier weer door met lezen!
EvaKrolis 20:11fe0aa7f111 354 prefx = 1*xMove; //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
EvaKrolis 20:11fe0aa7f111 355 // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
EvaKrolis 20:11fe0aa7f111 356 prefy = 1*yMove; //sw2,
Ramonwaninge 12:8d3bc1fa2321 357 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 358 }
Ramonwaninge 12:8d3bc1fa2321 359
EvaKrolis 16:deb42ce3c3a1 360 void printvalue()
EvaKrolis 16:deb42ce3c3a1 361 {
EvaKrolis 16:deb42ce3c3a1 362 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 363
EvaKrolis 16:deb42ce3c3a1 364 }
Ramonwaninge 12:8d3bc1fa2321 365
Ramonwaninge 15:38258e6b6e91 366 //State Machine
Ramonwaninge 15:38258e6b6e91 367 void StateMachine()
Ramonwaninge 15:38258e6b6e91 368 {
Ramonwaninge 15:38258e6b6e91 369 switch(CurrentState) { //Determine in which state you are
Ramonwaninge 15:38258e6b6e91 370
Ramonwaninge 15:38258e6b6e91 371 case Calibration: //Calibration mode
Ramonwaninge 15:38258e6b6e91 372 if(StateBool) { //If you go into this state
Ramonwaninge 15:38258e6b6e91 373 pc.printf("You can start calibrating. \n\r"); //Print that you are in this state
Ramonwaninge 15:38258e6b6e91 374 StateBool = false; //Set the bool for the start of a state to false
Ramonwaninge 15:38258e6b6e91 375 EMGCalibration0_timer.attach(&CalibrateEMG0,0.005); //Start EMG calibration every 0.005 seconds
Ramonwaninge 15:38258e6b6e91 376 EMGCalibration1_timer.attach(&CalibrateEMG1,0.005); //Start EMG calibration every 0.005 seconds
Ramonwaninge 15:38258e6b6e91 377 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 378 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 379 SwitchState_timer.attach(&SwitchState,20); //Switch to the next state after 16 seconds
Ramonwaninge 15:38258e6b6e91 380 blueled = 0;
Ramonwaninge 15:38258e6b6e91 381 }
Ramonwaninge 15:38258e6b6e91 382
Ramonwaninge 15:38258e6b6e91 383 if (SwitchStateBool) { //If the bool is changed
Ramonwaninge 15:38258e6b6e91 384 CurrentState = WorkingMode; //Change the state to the working mode
Ramonwaninge 15:38258e6b6e91 385 StateBool = true; //Set the start of a state bool to true
Ramonwaninge 15:38258e6b6e91 386 SwitchStateBool = false; //Set the switch bool to false
Ramonwaninge 15:38258e6b6e91 387 }
Ramonwaninge 15:38258e6b6e91 388 break;
Ramonwaninge 15:38258e6b6e91 389
Ramonwaninge 15:38258e6b6e91 390 case WorkingMode: //Mode to get the robot working
Ramonwaninge 15:38258e6b6e91 391 if (StateBool) { //If you start to go in this state
Ramonwaninge 15:38258e6b6e91 392 pc.printf("You are know in the working mode. \r\n"); //Print in which mode you are
Ramonwaninge 15:38258e6b6e91 393 StateBool = false; //Set the start of state bool to true
Ramonwaninge 15:38258e6b6e91 394 EMGCalibration0_timer.detach(); //Detach the the calibration
Ramonwaninge 15:38258e6b6e91 395 EMGCalibration1_timer.detach(); //Detach the calibration
Ramonwaninge 15:38258e6b6e91 396 ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005); //Start the use of EMG
Ramonwaninge 15:38258e6b6e91 397 ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005); //Start the use of EMG
EvaKrolis 20:11fe0aa7f111 398 kin.attach(kinematics, 0.008); // roep de ticker aan (
EvaKrolis 20:11fe0aa7f111 399 simulateval.attach(printvalue, 0.1);
Ramonwaninge 15:38258e6b6e91 400 }
Ramonwaninge 15:38258e6b6e91 401 blueled = 1; //Set the blue led off
EvaKrolis 16:deb42ce3c3a1 402 //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
Ramonwaninge 15:38258e6b6e91 403 break;
Ramonwaninge 15:38258e6b6e91 404 }
Ramonwaninge 15:38258e6b6e91 405 }
Ramonwaninge 15:38258e6b6e91 406
Ramonwaninge 0:779fe292e912 407 int main()
Ramonwaninge 0:779fe292e912 408 {
Ramonwaninge 12:8d3bc1fa2321 409 //Initial conditions
EvaKrolis 21:73e1cc927968 410 theta1 = PI*0.49; //Angle of the left motor
Ramonwaninge 12:8d3bc1fa2321 411 theta4 = PI*0.49;
Ramonwaninge 2:0a7a3c0c08d3 412 pc.baud(115200);
Ramonwaninge 13:f77c5f196161 413 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 414 blueled = 1;
Ramonwaninge 13:f77c5f196161 415 redled = 1;
Ramonwaninge 13:f77c5f196161 416 filter0.add(&Notch50_0).add(&High0); //Make filter chain for the first EMG
Ramonwaninge 13:f77c5f196161 417 filter1.add(&Notch50_1).add(&High1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 418 button.rise(StopProgram); //If the button is pressed, stop program
EvaKrolis 16:deb42ce3c3a1 419
Ramonwaninge 12:8d3bc1fa2321 420 while(true) {
Ramonwaninge 12:8d3bc1fa2321 421
Ramonwaninge 13:f77c5f196161 422 StateMachine(); //Start the state machine
Ramonwaninge 8:697aa3c94209 423 }
Ramonwaninge 0:779fe292e912 424 }