Ramon Waninge / Mbed 2 deprecated Bioroboticsmerge

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Eva Krolis

main.cpp@26:efd04dec7710, 2018-11-01 (annotated)

Committer:
Ramonwaninge
Date:
Thu Nov 01 11:00:13 2018 +0000
Revision:
26:efd04dec7710
Parent:
25:1123d100d964
Child:
27:22bfc75f8d1a
Cleaner code (opgeruimd)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 15:38258e6b6e91 1 //Libraries
Ramonwaninge 0:779fe292e912 2 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 3 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 4 #include <cmath>
Ramonwaninge 12:8d3bc1fa2321 5 #include "MODSERIAL.h"
Ramonwaninge 13:f77c5f196161 6 #include "BiQuad.h"
Ramonwaninge 13:f77c5f196161 7 #include <algorithm>
Ramonwaninge 25:1123d100d964 8 //#include "FastPWM.h"
Ramonwaninge 0:779fe292e912 9 #define PI 3.14159265
Ramonwaninge 14:e3fe54f0a4b4 10
Ramonwaninge 14:e3fe54f0a4b4 11 //Inputs and outputs
EvaKrolis 20:11fe0aa7f111 12 MODSERIAL pc(USBTX, USBRX); //Connecting to PC
Ramonwaninge 14:e3fe54f0a4b4 13 AnalogIn EMG0In(A0); //EMG input 0
Ramonwaninge 14:e3fe54f0a4b4 14 AnalogIn EMG1In(A1); //EMG input 1
Ramonwaninge 14:e3fe54f0a4b4 15 InterruptIn button(SW3); //Define button
Ramonwaninge 14:e3fe54f0a4b4 16 DigitalOut greenled(LED_GREEN); //Green led
Ramonwaninge 14:e3fe54f0a4b4 17 DigitalOut blueled(LED_BLUE); //Blue led
Ramonwaninge 14:e3fe54f0a4b4 18 DigitalOut redled(LED_RED); //Red led
Ramonwaninge 13:f77c5f196161 19
Ramonwaninge 15:38258e6b6e91 20 //Constants
Ramonwaninge 25:1123d100d964 21 const float ll = 230; //Length of the lower arm
Ramonwaninge 25:1123d100d964 22 const float lu = 198; //Length of the upper arm
Ramonwaninge 25:1123d100d964 23 const float lb = 50; //Length of the part between the upper arms
Ramonwaninge 25:1123d100d964 24 const float le = 79; //Length of the end-effector beam
Ramonwaninge 25:1123d100d964 25 const float xbase = 450-lb; //Length between the motors
Ramonwaninge 15:38258e6b6e91 26 const int Length = 10000; //Length of the array for the calibration
Ramonwaninge 15:38258e6b6e91 27 const int Parts = 50; //Mean average filter over 50 values
Ramonwaninge 15:38258e6b6e91 28
Ramonwaninge 14:e3fe54f0a4b4 29 //parameters for kinematics
Ramonwaninge 25:1123d100d964 30 float theta1 = PI*0.49; //Angle of the left motor
Ramonwaninge 25:1123d100d964 31 float theta4 = PI*0.49; //Angle of the right motor
Ramonwaninge 25:1123d100d964 32 float thetaflip = 0; //Angle of the flipping motor
Ramonwaninge 25:1123d100d964 33 float prefx; //Desired x velocity
Ramonwaninge 25:1123d100d964 34 float prefy; //Desired y velocity "
Ramonwaninge 26:efd04dec7710 35 float dt = 0.002; //Time step of the system
Ramonwaninge 26:efd04dec7710 36 float iJ[2][2]; //inverse Jacobian matrix
Ramonwaninge 25:1123d100d964 37 //Time step (dependent on sample frequency)
EvaKrolis 20:11fe0aa7f111 38
EvaKrolis 20:11fe0aa7f111 39 //Kinematics parameters for x
Ramonwaninge 25:1123d100d964 40 float xendsum;
Ramonwaninge 25:1123d100d964 41 float xendsqrt1;
Ramonwaninge 25:1123d100d964 42 float xendsqrt2;
Ramonwaninge 25:1123d100d964 43 float xend;
Ramonwaninge 25:1123d100d964 44 float jacobiana;
Ramonwaninge 25:1123d100d964 45 float jacobianc;
EvaKrolis 20:11fe0aa7f111 46
EvaKrolis 20:11fe0aa7f111 47 //Kinematics parameters for y
Ramonwaninge 25:1123d100d964 48 float yendsum;
Ramonwaninge 25:1123d100d964 49 float yendsqrt1;
Ramonwaninge 25:1123d100d964 50 float yendsqrt2;
Ramonwaninge 25:1123d100d964 51 float yend;
Ramonwaninge 25:1123d100d964 52 float jacobianb;
Ramonwaninge 25:1123d100d964 53 float jacobiand;
Ramonwaninge 14:e3fe54f0a4b4 54
Ramonwaninge 14:e3fe54f0a4b4 55 //Parameters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 56 float EMG0; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 57 float EMG0filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 58 float EMG0filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 59 float EMG0Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 60 float Sum0 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 61 float EMG0Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 62 int ReadCal0 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 63 float MaxValue0 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 64 float Threshold0 = 0; //Threshold for the first EMG signal
Ramonwaninge 13:f77c5f196161 65
Ramonwaninge 14:e3fe54f0a4b4 66 //Parameters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 67 float EMG1; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 68 float EMG1filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 69 float EMG1filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 70 float EMG1Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 71 float Sum1 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 72 float EMG1Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 73 int ReadCal1 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 74 float MaxValue1 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 75 float Threshold1 = 0; //Threshold for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 76
Ramonwaninge 14:e3fe54f0a4b4 77 //Filter variables
Ramonwaninge 14:e3fe54f0a4b4 78 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
Ramonwaninge 14:e3fe54f0a4b4 79 BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
Ramonwaninge 14:e3fe54f0a4b4 80 BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
Ramonwaninge 14:e3fe54f0a4b4 81 BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
Ramonwaninge 14:e3fe54f0a4b4 82 BiQuadChain filter0; //Make chain of filters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 83 BiQuadChain filter1; //Make chain of filters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 84
Ramonwaninge 13:f77c5f196161 85 //Timers and Tickers
Ramonwaninge 13:f77c5f196161 86 Ticker kin; //Timer for calculating x,y,theta1,theta4
Ramonwaninge 13:f77c5f196161 87 Ticker simulateval; //Timer that prints the values for x,y, and angles
Ramonwaninge 13:f77c5f196161 88 Ticker ReadUseEMG0_timer; //Timer to read, filter and use the EMG
Ramonwaninge 15:38258e6b6e91 89 Ticker EMGCalibration0_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 90 Ticker FindMax0_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 91 Ticker ReadUseEMG1_timer; //Timer to read, filter and use the EMG
Ramonwaninge 15:38258e6b6e91 92 Ticker EMGCalibration1_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 93 Ticker FindMax1_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 94 Ticker SwitchState_timer; //Timer to switch from the Calibration to the working mode
Ramonwaninge 25:1123d100d964 95 Timer local_timer;
Ramonwaninge 13:f77c5f196161 96
Ramonwaninge 14:e3fe54f0a4b4 97 //Bool for movement
Ramonwaninge 14:e3fe54f0a4b4 98 bool xMove = false; //Bool for the x-movement
Ramonwaninge 14:e3fe54f0a4b4 99 bool yMove = false; //Bool for the y-movement
Ramonwaninge 14:e3fe54f0a4b4 100
Ramonwaninge 14:e3fe54f0a4b4 101 //Parameters for the state machine
Ramonwaninge 14:e3fe54f0a4b4 102 enum States {Calibration, WorkingMode}; //Initialize state machine
EvaKrolis 20:11fe0aa7f111 103 States CurrentState = Calibration; //Start in the calibration mode
Ramonwaninge 14:e3fe54f0a4b4 104 bool StateBool = true; //Bool to first go in a state
Ramonwaninge 14:e3fe54f0a4b4 105 bool SwitchStateBool = false; //Bool to switch from calibration to working mode
Ramonwaninge 14:e3fe54f0a4b4 106
Ramonwaninge 14:e3fe54f0a4b4 107 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 108 void ReadUseEMG0()
Ramonwaninge 15:38258e6b6e91 109 {
Ramonwaninge 15:38258e6b6e91 110 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 14:e3fe54f0a4b4 111 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 14:e3fe54f0a4b4 112 }
Ramonwaninge 15:38258e6b6e91 113
Ramonwaninge 15:38258e6b6e91 114 Sum0 = 0;
Ramonwaninge 15:38258e6b6e91 115 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 116 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 15:38258e6b6e91 117 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 118 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 119
Ramonwaninge 15:38258e6b6e91 120 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 121 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 122 }
Ramonwaninge 15:38258e6b6e91 123 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 124
Ramonwaninge 15:38258e6b6e91 125 if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
Ramonwaninge 25:1123d100d964 126 //redled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 127 xMove = true; //Set movement to true
Ramonwaninge 15:38258e6b6e91 128 } else {
Ramonwaninge 25:1123d100d964 129 //redled = 1; //Otherwise turn the LED off
Ramonwaninge 15:38258e6b6e91 130 xMove = false; //Otherwise set movement to false
Ramonwaninge 15:38258e6b6e91 131 }
Ramonwaninge 15:38258e6b6e91 132 }
Ramonwaninge 15:38258e6b6e91 133 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 134 void ReadUseEMG1()
Ramonwaninge 15:38258e6b6e91 135 {
Ramonwaninge 15:38258e6b6e91 136 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 137 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 138 }
Ramonwaninge 15:38258e6b6e91 139
Ramonwaninge 15:38258e6b6e91 140 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 141 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 142 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 143 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 144 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 145
Ramonwaninge 15:38258e6b6e91 146 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 147 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 148 }
Ramonwaninge 15:38258e6b6e91 149 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 150
Ramonwaninge 15:38258e6b6e91 151 if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
Ramonwaninge 15:38258e6b6e91 152 greenled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 153 yMove = true; //Set y movement to true
Ramonwaninge 15:38258e6b6e91 154 } else {
Ramonwaninge 15:38258e6b6e91 155 greenled = 1; //Otherwise turn the LED off
EvaKrolis 16:deb42ce3c3a1 156 yMove = false; //Otherwise set y movement to false
Ramonwaninge 15:38258e6b6e91 157 }
Ramonwaninge 15:38258e6b6e91 158 }
Ramonwaninge 15:38258e6b6e91 159
Ramonwaninge 15:38258e6b6e91 160
Ramonwaninge 15:38258e6b6e91 161 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 162 void CalibrateEMG0()
Ramonwaninge 15:38258e6b6e91 163 {
Ramonwaninge 15:38258e6b6e91 164 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 165 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 166 }
Ramonwaninge 15:38258e6b6e91 167
Ramonwaninge 14:e3fe54f0a4b4 168 Sum0 = 0;
Ramonwaninge 14:e3fe54f0a4b4 169 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 14:e3fe54f0a4b4 170 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 14:e3fe54f0a4b4 171 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 14:e3fe54f0a4b4 172 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 173
Ramonwaninge 15:38258e6b6e91 174 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 14:e3fe54f0a4b4 175 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 176 }
Ramonwaninge 15:38258e6b6e91 177 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 178
Ramonwaninge 15:38258e6b6e91 179 ReadCal0++;
Ramonwaninge 15:38258e6b6e91 180 }
Ramonwaninge 15:38258e6b6e91 181
Ramonwaninge 15:38258e6b6e91 182 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 183 void CalibrateEMG1()
Ramonwaninge 15:38258e6b6e91 184 {
Ramonwaninge 15:38258e6b6e91 185 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 186 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 187 }
Ramonwaninge 15:38258e6b6e91 188
Ramonwaninge 15:38258e6b6e91 189 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 190 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 191 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 192 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 193 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 194
Ramonwaninge 15:38258e6b6e91 195 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 196 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 197 }
Ramonwaninge 15:38258e6b6e91 198 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 199
Ramonwaninge 15:38258e6b6e91 200 ReadCal1++;
Ramonwaninge 15:38258e6b6e91 201 }
Ramonwaninge 15:38258e6b6e91 202
Ramonwaninge 15:38258e6b6e91 203 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 204 void FindMax0()
Ramonwaninge 15:38258e6b6e91 205 {
Ramonwaninge 15:38258e6b6e91 206 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 207 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 208 pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
Ramonwaninge 15:38258e6b6e91 209 FindMax0_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 210 }
Ramonwaninge 15:38258e6b6e91 211
Ramonwaninge 15:38258e6b6e91 212 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 213 void FindMax1()
Ramonwaninge 15:38258e6b6e91 214 {
Ramonwaninge 15:38258e6b6e91 215 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 216 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 217 pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
Ramonwaninge 15:38258e6b6e91 218 FindMax1_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 219 }
Ramonwaninge 15:38258e6b6e91 220
Ramonwaninge 15:38258e6b6e91 221 //Function to stop the reading of the EMG
Ramonwaninge 15:38258e6b6e91 222 void StopProgram()
Ramonwaninge 15:38258e6b6e91 223 {
Ramonwaninge 15:38258e6b6e91 224 greenled = 1; //Turn the LEDs off
Ramonwaninge 15:38258e6b6e91 225 blueled = 1;
Ramonwaninge 15:38258e6b6e91 226 redled = 1;
Ramonwaninge 15:38258e6b6e91 227 exit (0); //Abort mission!!
Ramonwaninge 15:38258e6b6e91 228 }
Ramonwaninge 15:38258e6b6e91 229
Ramonwaninge 15:38258e6b6e91 230 //Function to switch a state
Ramonwaninge 25:1123d100d964 231 /*void SwitchState()
Ramonwaninge 15:38258e6b6e91 232 {
Ramonwaninge 15:38258e6b6e91 233 SwitchStateBool = true; //Set the bool for the start of a state to true
Ramonwaninge 15:38258e6b6e91 234 SwitchState_timer.detach(); //Use this function once
Ramonwaninge 25:1123d100d964 235 }*/
Ramonwaninge 14:e3fe54f0a4b4 236
Ramonwaninge 26:efd04dec7710 237 //forward kinematics function , &xend and&yend are output.
Ramonwaninge 25:1123d100d964 238 void FK(float &xend_, float &yend_, float theta1_, float theta4_)
Ramonwaninge 25:1123d100d964 239 {
Ramonwaninge 26:efd04dec7710 240
Ramonwaninge 26:efd04dec7710 241 //Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output
Ramonwaninge 25:1123d100d964 242 float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
Ramonwaninge 25:1123d100d964 243 float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
Ramonwaninge 25:1123d100d964 244 float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
Ramonwaninge 25:1123d100d964 245 xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
Ramonwaninge 26:efd04dec7710 246
Ramonwaninge 25:1123d100d964 247 float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
Ramonwaninge 25:1123d100d964 248 float yendsqrt1_ = (-xbase/ll + cos(theta1_)+cos(theta4_))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1_)+cos(theta4_))- ll*(1+cos(theta1_+theta4_))));
Ramonwaninge 25:1123d100d964 249 float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
Ramonwaninge 25:1123d100d964 250 yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
Ramonwaninge 25:1123d100d964 251 }
Ramonwaninge 14:e3fe54f0a4b4 252
Ramonwaninge 26:efd04dec7710 253 //Below we have the inverse kinematics function.
Ramonwaninge 25:1123d100d964 254 void inverse(float prex, float prey)
Ramonwaninge 15:38258e6b6e91 255 {
Ramonwaninge 26:efd04dec7710 256 theta1 += (prefx*(iJ[0][0])+iJ[0][1]*prey)*dt; //theta 1 is itself + the desired speeds in x and y direction, both
Ramonwaninge 26:efd04dec7710 257 theta4 += (prefx*iJ[1][0]+iJ[1][1]*prey)*dt; //multiplied with a prefactor which comes out of the motor
Ramonwaninge 26:efd04dec7710 258 //the iJ values are defined in the "kinematics" function
Ramonwaninge 26:efd04dec7710 259
Ramonwaninge 26:efd04dec7710 260 //Calling the forward kinematics, to calculate xend and yend
Ramonwaninge 25:1123d100d964 261 FK(xend,yend,theta1,theta4);
Ramonwaninge 12:8d3bc1fa2321 262
Ramonwaninge 3:de8d3ca44a3e 263 }
Ramonwaninge 25:1123d100d964 264
Ramonwaninge 12:8d3bc1fa2321 265 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 266 {
Ramonwaninge 12:8d3bc1fa2321 267
Ramonwaninge 25:1123d100d964 268 float xend1,xend2,xend3,yend1,yend2,yend3;
Ramonwaninge 25:1123d100d964 269 const float dq = 0.0001;
Ramonwaninge 25:1123d100d964 270 FK(xend1,yend1,theta1,theta4);
Ramonwaninge 25:1123d100d964 271 FK(xend2,yend2,theta1+dq,theta4);
Ramonwaninge 25:1123d100d964 272 FK(xend3,yend3,theta1,theta4+dq);
Ramonwaninge 25:1123d100d964 273
Ramonwaninge 25:1123d100d964 274 float a,b,c,d;
Ramonwaninge 25:1123d100d964 275 a = xend2-xend1;
Ramonwaninge 25:1123d100d964 276 b = xend3-xend1;
Ramonwaninge 25:1123d100d964 277 c = yend2-yend1;
Ramonwaninge 25:1123d100d964 278 d = yend3-yend1;
Ramonwaninge 25:1123d100d964 279
Ramonwaninge 25:1123d100d964 280 float Q = 1/(a*d-b*c)/dq;
Ramonwaninge 25:1123d100d964 281
Ramonwaninge 25:1123d100d964 282
Ramonwaninge 25:1123d100d964 283 iJ[0][0] = d*Q;
Ramonwaninge 25:1123d100d964 284 iJ[0][1]= -c*Q;
Ramonwaninge 25:1123d100d964 285 iJ[1][0] = -b*Q;
Ramonwaninge 25:1123d100d964 286 iJ[1][1] = a*Q;
Ramonwaninge 26:efd04dec7710 287
Ramonwaninge 26:efd04dec7710 288
Ramonwaninge 26:efd04dec7710 289 prefx = 1*xMove; //sw3, Prefx has multiplier one, but that has to become a value dependant on the motor
Ramonwaninge 26:efd04dec7710 290 prefy = 1*yMove; //sw2,
Ramonwaninge 12:8d3bc1fa2321 291 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 292 }
Ramonwaninge 12:8d3bc1fa2321 293
Ramonwaninge 26:efd04dec7710 294 // these values are printed for controlling purposes (can later be removed)
EvaKrolis 16:deb42ce3c3a1 295 void printvalue()
EvaKrolis 16:deb42ce3c3a1 296 {
Ramonwaninge 26:efd04dec7710 297 pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 = %f",xend, yend,theta1,theta4);
Ramonwaninge 26:efd04dec7710 298 }
Ramonwaninge 12:8d3bc1fa2321 299
Ramonwaninge 15:38258e6b6e91 300 //State Machine
Ramonwaninge 15:38258e6b6e91 301 void StateMachine()
Ramonwaninge 15:38258e6b6e91 302 {
Ramonwaninge 25:1123d100d964 303 redled = 1;
Ramonwaninge 15:38258e6b6e91 304 switch(CurrentState) { //Determine in which state you are
Ramonwaninge 15:38258e6b6e91 305
Ramonwaninge 25:1123d100d964 306 case Calibration:
Ramonwaninge 25:1123d100d964 307 //Calibration mode
Ramonwaninge 15:38258e6b6e91 308 if(StateBool) { //If you go into this state
Ramonwaninge 15:38258e6b6e91 309 pc.printf("You can start calibrating. \n\r"); //Print that you are in this state
Ramonwaninge 15:38258e6b6e91 310 StateBool = false; //Set the bool for the start of a state to false
Ramonwaninge 15:38258e6b6e91 311 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 312 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
Ramonwaninge 25:1123d100d964 313 //SwitchState_timer.attach(&SwitchState,20); //Switch to the next state after 16 seconds
Ramonwaninge 25:1123d100d964 314 local_timer.reset();
Ramonwaninge 25:1123d100d964 315 local_timer.start();
Ramonwaninge 15:38258e6b6e91 316 blueled = 0;
Ramonwaninge 15:38258e6b6e91 317 }
Ramonwaninge 15:38258e6b6e91 318
Ramonwaninge 26:efd04dec7710 319 CalibrateEMG0(); //start emg calibration every 0.005 seconds
Ramonwaninge 25:1123d100d964 320 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
Ramonwaninge 25:1123d100d964 321
Ramonwaninge 25:1123d100d964 322 if (local_timer.read() > 20) { //If the bool is changed
Ramonwaninge 15:38258e6b6e91 323 CurrentState = WorkingMode; //Change the state to the working mode
Ramonwaninge 15:38258e6b6e91 324 StateBool = true; //Set the start of a state bool to true
Ramonwaninge 25:1123d100d964 325 //SwitchStateBool = false; //Set the switch bool to false
Ramonwaninge 15:38258e6b6e91 326 }
Ramonwaninge 15:38258e6b6e91 327 break;
Ramonwaninge 15:38258e6b6e91 328
Ramonwaninge 15:38258e6b6e91 329 case WorkingMode: //Mode to get the robot working
Ramonwaninge 15:38258e6b6e91 330 if (StateBool) { //If you start to go in this state
Ramonwaninge 15:38258e6b6e91 331 pc.printf("You are know in the working mode. \r\n"); //Print in which mode you are
Ramonwaninge 15:38258e6b6e91 332 StateBool = false; //Set the start of state bool to true
Ramonwaninge 25:1123d100d964 333 //ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005); //Start the use of EMG
Ramonwaninge 25:1123d100d964 334 //ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005); //Start the use of EMG
Ramonwaninge 25:1123d100d964 335 //kin.attach(kinematics, 0.005); // roep de ticker aan (
Ramonwaninge 25:1123d100d964 336 //simulateval.attach(printvalue, 0.1);
Ramonwaninge 15:38258e6b6e91 337 }
Ramonwaninge 25:1123d100d964 338 blueled = 1;
Ramonwaninge 25:1123d100d964 339
Ramonwaninge 26:efd04dec7710 340 ReadUseEMG0();//Start the use of EMG
Ramonwaninge 26:efd04dec7710 341 ReadUseEMG1();//Start the use of EMG
Ramonwaninge 26:efd04dec7710 342 kinematics(); //Starts calculating the x and y value of the endeffector, as well as the desired values and their BIJBEHORENDE angles
Ramonwaninge 25:1123d100d964 343
Ramonwaninge 25:1123d100d964 344 //motorcontroller
Ramonwaninge 25:1123d100d964 345 //Set the blue led off
EvaKrolis 16:deb42ce3c3a1 346 //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
Ramonwaninge 15:38258e6b6e91 347 break;
Ramonwaninge 15:38258e6b6e91 348 }
Ramonwaninge 25:1123d100d964 349 redled = 0;
Ramonwaninge 15:38258e6b6e91 350 }
Ramonwaninge 15:38258e6b6e91 351
Ramonwaninge 0:779fe292e912 352 int main()
Ramonwaninge 0:779fe292e912 353 {
Ramonwaninge 25:1123d100d964 354 Ticker hoofdticker;
Ramonwaninge 12:8d3bc1fa2321 355 //Initial conditions
EvaKrolis 21:73e1cc927968 356 theta1 = PI*0.49; //Angle of the left motor
Ramonwaninge 12:8d3bc1fa2321 357 theta4 = PI*0.49;
Ramonwaninge 26:efd04dec7710 358 FK(xend,yend,theta1,theta4);
Ramonwaninge 2:0a7a3c0c08d3 359 pc.baud(115200);
Ramonwaninge 13:f77c5f196161 360 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 361 blueled = 1;
Ramonwaninge 13:f77c5f196161 362 redled = 1;
Ramonwaninge 13:f77c5f196161 363 filter0.add(&Notch50_0).add(&High0); //Make filter chain for the first EMG
Ramonwaninge 13:f77c5f196161 364 filter1.add(&Notch50_1).add(&High1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 365 button.rise(StopProgram); //If the button is pressed, stop program
Ramonwaninge 25:1123d100d964 366 hoofdticker.attach(&StateMachine,0.002);
Ramonwaninge 12:8d3bc1fa2321 367 while(true) {
Ramonwaninge 25:1123d100d964 368 printvalue();
Ramonwaninge 25:1123d100d964 369 wait(0.75);
Ramonwaninge 25:1123d100d964 370 //Start the state machine
Ramonwaninge 8:697aa3c94209 371 }
Ramonwaninge 0:779fe292e912 372 }