Joost Herijgers / Mbed 2 deprecated Code_MotorEMG

EMG and motor script together, Not fully working yet,

Dependencies:   Encoder QEI biquadFilter mbed

main.cpp@0:eb16ed402ffa, 2017-10-25 (annotated)

Committer:
Joost38H
Date:
Wed Oct 25 10:40:18 2017 +0000
Revision:
0:eb16ed402ffa
Child:
1:6aac013b0ba3
Net gemaakte versie van de code, zonder EMG nog

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joost38H 0:eb16ed402ffa 1 #include "mbed.h"
Joost38H 0:eb16ed402ffa 2 #include "math.h"
Joost38H 0:eb16ed402ffa 3 #include "encoder.h"
Joost38H 0:eb16ed402ffa 4 #include "QEI.h"
Joost38H 0:eb16ed402ffa 5 #include "BiQuad.h"
Joost38H 0:eb16ed402ffa 6
Joost38H 0:eb16ed402ffa 7 Serial pc(USBTX, USBRX);
Joost38H 0:eb16ed402ffa 8
Joost38H 0:eb16ed402ffa 9 //Defining all in- and outputs
Joost38H 0:eb16ed402ffa 10 //EMG input
Joost38H 0:eb16ed402ffa 11 AnalogIn emgBR( A0 ); //Right Biceps
Joost38H 0:eb16ed402ffa 12 AnalogIn emgBL( A1 ); //Left Biceps
Joost38H 0:eb16ed402ffa 13
Joost38H 0:eb16ed402ffa 14 //Output motor 1 and reading Encoder motor 1
Joost38H 0:eb16ed402ffa 15 DigitalOut motor1DirectionPin(D4);
Joost38H 0:eb16ed402ffa 16 PwmOut motor1MagnitudePin(D5);
Joost38H 0:eb16ed402ffa 17 QEI Encoder1(D12,D13,NC,32);
Joost38H 0:eb16ed402ffa 18
Joost38H 0:eb16ed402ffa 19
Joost38H 0:eb16ed402ffa 20 //Output motor 2 and reading Encoder motor 2
Joost38H 0:eb16ed402ffa 21 DigitalOut motor2DirectionPin(D7);
Joost38H 0:eb16ed402ffa 22 PwmOut motor2MagnitudePin(D6);
Joost38H 0:eb16ed402ffa 23 QEI Encoder2(D10,D11,NC,32);
Joost38H 0:eb16ed402ffa 24
Joost38H 0:eb16ed402ffa 25 //LED output, needed for feedback
Joost38H 0:eb16ed402ffa 26 DigitalOut led_R(LED_RED);
Joost38H 0:eb16ed402ffa 27 DigitalOut led_G(LED_GREEN);
Joost38H 0:eb16ed402ffa 28 DigitalOut led_B(LED_BLUE);
Joost38H 0:eb16ed402ffa 29
Joost38H 0:eb16ed402ffa 30 InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG
Joost38H 0:eb16ed402ffa 31
Joost38H 0:eb16ed402ffa 32 Timer t;
Joost38H 0:eb16ed402ffa 33 double speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo?
Joost38H 0:eb16ed402ffa 34
Joost38H 0:eb16ed402ffa 35 // Getting the counts from the Encoder
Joost38H 0:eb16ed402ffa 36 int counts1 = Encoder1.getPulses();
Joost38H 0:eb16ed402ffa 37 int counts2 = Encoder2.getPulses();
Joost38H 0:eb16ed402ffa 38
Joost38H 0:eb16ed402ffa 39 // Defining variables delta (the difference between position and desired position) <- Is dit zo?
Joost38H 0:eb16ed402ffa 40 int delta1;
Joost38H 0:eb16ed402ffa 41 int delta2;
Joost38H 0:eb16ed402ffa 42
Joost38H 0:eb16ed402ffa 43 // Initial input value for X
Joost38H 0:eb16ed402ffa 44 int Xin=0; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
Joost38H 0:eb16ed402ffa 45 double huidigetijdX;
Joost38H 0:eb16ed402ffa 46
Joost38H 0:eb16ed402ffa 47 // Feedback system for counting values of X
Joost38H 0:eb16ed402ffa 48 void ledtX(){
Joost38H 0:eb16ed402ffa 49 t.reset();
Joost38H 0:eb16ed402ffa 50 Xin++;
Joost38H 0:eb16ed402ffa 51 pc.printf("Xin is %i\n",Xin);
Joost38H 0:eb16ed402ffa 52 led_G=0;
Joost38H 0:eb16ed402ffa 53 led_R=1;
Joost38H 0:eb16ed402ffa 54 wait(0.5);
Joost38H 0:eb16ed402ffa 55 led_G=1;
Joost38H 0:eb16ed402ffa 56 led_R=0;
Joost38H 0:eb16ed402ffa 57 }
Joost38H 0:eb16ed402ffa 58
Joost38H 0:eb16ed402ffa 59
Joost38H 0:eb16ed402ffa 60 // Couting system for values of X
Joost38H 0:eb16ed402ffa 61 int tellerX(){
Joost38H 0:eb16ed402ffa 62 led_G=1;
Joost38H 0:eb16ed402ffa 63 led_B=1;
Joost38H 0:eb16ed402ffa 64 while(true){
Joost38H 0:eb16ed402ffa 65 button.fall(ledtX); //This has to be replaced by EMG
Joost38H 0:eb16ed402ffa 66 /*if (EMG>threshold){
Joost38H 0:eb16ed402ffa 67 ledtX(); // dit is wat je uiteindelijk wil dat er staat
Joost38H 0:eb16ed402ffa 68 }*/
Joost38H 0:eb16ed402ffa 69 t.start();
Joost38H 0:eb16ed402ffa 70 huidigetijdX=t.read();
Joost38H 0:eb16ed402ffa 71 if (huidigetijdX>2){
Joost38H 0:eb16ed402ffa 72 led_R=1; //Go to the next program (couting values for Y)
Joost38H 0:eb16ed402ffa 73 return 0;
Joost38H 0:eb16ed402ffa 74 }
Joost38H 0:eb16ed402ffa 75 }
Joost38H 0:eb16ed402ffa 76 }
Joost38H 0:eb16ed402ffa 77
Joost38H 0:eb16ed402ffa 78 // Initial values needed for Y (see comments at X function)
Joost38H 0:eb16ed402ffa 79 int Yin=0;
Joost38H 0:eb16ed402ffa 80 double huidigetijdY;
Joost38H 0:eb16ed402ffa 81
Joost38H 0:eb16ed402ffa 82 //Feedback system for couting values of Y
Joost38H 0:eb16ed402ffa 83 void ledtY(){
Joost38H 0:eb16ed402ffa 84 t.reset();
Joost38H 0:eb16ed402ffa 85 Yin++;
Joost38H 0:eb16ed402ffa 86 pc.printf("Yin is %i\n",Yin);
Joost38H 0:eb16ed402ffa 87 led_G=0;
Joost38H 0:eb16ed402ffa 88 led_B=1;
Joost38H 0:eb16ed402ffa 89 wait(0.5);
Joost38H 0:eb16ed402ffa 90 led_G=1;
Joost38H 0:eb16ed402ffa 91 led_B=0;
Joost38H 0:eb16ed402ffa 92 }
Joost38H 0:eb16ed402ffa 93
Joost38H 0:eb16ed402ffa 94 // Couting system for values of Y
Joost38H 0:eb16ed402ffa 95 int tellerY(){
Joost38H 0:eb16ed402ffa 96 t.reset();
Joost38H 0:eb16ed402ffa 97 led_G=1;
Joost38H 0:eb16ed402ffa 98 led_B=0;
Joost38H 0:eb16ed402ffa 99 while(true){
Joost38H 0:eb16ed402ffa 100 button.fall(ledtY); //See comments at X
Joost38H 0:eb16ed402ffa 101 /*if (EMG>threshold){
Joost38H 0:eb16ed402ffa 102 Piek(); // dit is wat je uiteindelijk wil dat er staat
Joost38H 0:eb16ed402ffa 103 }*/
Joost38H 0:eb16ed402ffa 104 t.start();
Joost38H 0:eb16ed402ffa 105 huidigetijdY=t.read();
Joost38H 0:eb16ed402ffa 106 if (huidigetijdY>2){
Joost38H 0:eb16ed402ffa 107 led_B=1;
Joost38H 0:eb16ed402ffa 108 button.fall(0);
Joost38H 0:eb16ed402ffa 109 return 0; // ga door naar het volgende programma
Joost38H 0:eb16ed402ffa 110 }
Joost38H 0:eb16ed402ffa 111 }
Joost38H 0:eb16ed402ffa 112 }
Joost38H 0:eb16ed402ffa 113
Joost38H 0:eb16ed402ffa 114 bool B = false;
Joost38H 0:eb16ed402ffa 115
Joost38H 0:eb16ed402ffa 116 // Declaring all variables needed for calculating rope lengths,
Joost38H 0:eb16ed402ffa 117 double Pox = 0;
Joost38H 0:eb16ed402ffa 118 double Poy = 0;
Joost38H 0:eb16ed402ffa 119 double Pbx = 0;
Joost38H 0:eb16ed402ffa 120 double Pby = 887;
Joost38H 0:eb16ed402ffa 121 double Prx = 768;
Joost38H 0:eb16ed402ffa 122 double Pry = 443;
Joost38H 0:eb16ed402ffa 123 double Pex=121;
Joost38H 0:eb16ed402ffa 124 double Pey=308;
Joost38H 0:eb16ed402ffa 125 double diamtrklosje=20;
Joost38H 0:eb16ed402ffa 126 double pi=3.14159265359;
Joost38H 0:eb16ed402ffa 127 double omtrekklosje=diamtrklosje*pi;
Joost38H 0:eb16ed402ffa 128 double Lou;
Joost38H 0:eb16ed402ffa 129 double Lbu;
Joost38H 0:eb16ed402ffa 130 double Lru;
Joost38H 0:eb16ed402ffa 131 double dLod;
Joost38H 0:eb16ed402ffa 132 double dLbd;
Joost38H 0:eb16ed402ffa 133 double dLrd;
Joost38H 0:eb16ed402ffa 134
Joost38H 0:eb16ed402ffa 135 // Declaring variables needed for calculating motor counts
Joost38H 0:eb16ed402ffa 136 double roto;
Joost38H 0:eb16ed402ffa 137 double rotb;
Joost38H 0:eb16ed402ffa 138 double rotr;
Joost38H 0:eb16ed402ffa 139 double rotzo;
Joost38H 0:eb16ed402ffa 140 double rotzb;
Joost38H 0:eb16ed402ffa 141 double rotzr;
Joost38H 0:eb16ed402ffa 142 double counto;
Joost38H 0:eb16ed402ffa 143 double countb;
Joost38H 0:eb16ed402ffa 144 double countr;
Joost38H 0:eb16ed402ffa 145 double countzo;
Joost38H 0:eb16ed402ffa 146 double countzb;
Joost38H 0:eb16ed402ffa 147 double countzr;
Joost38H 0:eb16ed402ffa 148
Joost38H 0:eb16ed402ffa 149 // Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit?
Joost38H 0:eb16ed402ffa 150 double Psx;
Joost38H 0:eb16ed402ffa 151 double Psy;
Joost38H 0:eb16ed402ffa 152 double Vex;
Joost38H 0:eb16ed402ffa 153 double Vey;
Joost38H 0:eb16ed402ffa 154 double Kz=0.7; // nadersnelheid instellen
Joost38H 0:eb16ed402ffa 155 double modVe;
Joost38H 0:eb16ed402ffa 156 double Vmax=20;
Joost38H 0:eb16ed402ffa 157 double Pstx;
Joost38H 0:eb16ed402ffa 158 double Psty;
Joost38H 0:eb16ed402ffa 159 double T=0.02;//seconds
Joost38H 0:eb16ed402ffa 160
Joost38H 0:eb16ed402ffa 161
Joost38H 0:eb16ed402ffa 162 //Deel om motor(en) aan te sturen--------------------------------------------
Joost38H 0:eb16ed402ffa 163
Joost38H 0:eb16ed402ffa 164 void calcdelta1() {
Joost38H 0:eb16ed402ffa 165 delta1 = (counto - Encoder1.getPulses());
Joost38H 0:eb16ed402ffa 166 }
Joost38H 0:eb16ed402ffa 167
Joost38H 0:eb16ed402ffa 168 void calcdelta2() {
Joost38H 0:eb16ed402ffa 169 delta2 = (countb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is
Joost38H 0:eb16ed402ffa 170 }
Joost38H 0:eb16ed402ffa 171
Joost38H 0:eb16ed402ffa 172 double referenceVelocity1;
Joost38H 0:eb16ed402ffa 173 double motorValue1;
Joost38H 0:eb16ed402ffa 174
Joost38H 0:eb16ed402ffa 175 double referenceVelocity2;
Joost38H 0:eb16ed402ffa 176 double motorValue2;
Joost38H 0:eb16ed402ffa 177
Joost38H 0:eb16ed402ffa 178 Ticker controlmotor1; // één ticker van maken?
Joost38H 0:eb16ed402ffa 179 Ticker calculatedelta1;
Joost38H 0:eb16ed402ffa 180 Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 0:eb16ed402ffa 181
Joost38H 0:eb16ed402ffa 182 Ticker controlmotor2; // één ticker van maken?
Joost38H 0:eb16ed402ffa 183 Ticker calculatedelta2;
Joost38H 0:eb16ed402ffa 184 Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 0:eb16ed402ffa 185
Joost38H 0:eb16ed402ffa 186 double GetReferenceVelocity1()
Joost38H 0:eb16ed402ffa 187 {
Joost38H 0:eb16ed402ffa 188 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 0:eb16ed402ffa 189 double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 0:eb16ed402ffa 190 referenceVelocity1 = (-1)*speedfactor * maxVelocity1;
Joost38H 0:eb16ed402ffa 191
Joost38H 0:eb16ed402ffa 192 if (Encoder1.getPulses() < (counto+1))
Joost38H 0:eb16ed402ffa 193 { speedfactor = 0.1;
Joost38H 0:eb16ed402ffa 194 }
Joost38H 0:eb16ed402ffa 195 else if (Encoder1.getPulses() > (counto-1))
Joost38H 0:eb16ed402ffa 196 { speedfactor = -0.1;
Joost38H 0:eb16ed402ffa 197 }
Joost38H 0:eb16ed402ffa 198 else
Joost38H 0:eb16ed402ffa 199 { speedfactor = 0;
Joost38H 0:eb16ed402ffa 200 }
Joost38H 0:eb16ed402ffa 201
Joost38H 0:eb16ed402ffa 202 return referenceVelocity1;
Joost38H 0:eb16ed402ffa 203 }
Joost38H 0:eb16ed402ffa 204
Joost38H 0:eb16ed402ffa 205 double GetReferenceVelocity2()
Joost38H 0:eb16ed402ffa 206 {
Joost38H 0:eb16ed402ffa 207 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 0:eb16ed402ffa 208 double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 0:eb16ed402ffa 209 referenceVelocity2 = (-1)*speedfactor * maxVelocity2;
Joost38H 0:eb16ed402ffa 210
Joost38H 0:eb16ed402ffa 211 if (Encoder2.getPulses() < (counto+1))
Joost38H 0:eb16ed402ffa 212 { speedfactor = 0.1;
Joost38H 0:eb16ed402ffa 213 }
Joost38H 0:eb16ed402ffa 214 else if (Encoder2.getPulses() > (counto-1))
Joost38H 0:eb16ed402ffa 215 { speedfactor = -0.1;
Joost38H 0:eb16ed402ffa 216 }
Joost38H 0:eb16ed402ffa 217 else
Joost38H 0:eb16ed402ffa 218 { speedfactor = 0;
Joost38H 0:eb16ed402ffa 219 }
Joost38H 0:eb16ed402ffa 220
Joost38H 0:eb16ed402ffa 221 return referenceVelocity2;
Joost38H 0:eb16ed402ffa 222 }
Joost38H 0:eb16ed402ffa 223
Joost38H 0:eb16ed402ffa 224 void SetMotor1(double motorValue1)
Joost38H 0:eb16ed402ffa 225 {
Joost38H 0:eb16ed402ffa 226 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 0:eb16ed402ffa 227 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 0:eb16ed402ffa 228 if (motorValue1 >=0) motor1DirectionPin=1;
Joost38H 0:eb16ed402ffa 229 else motor1DirectionPin=0;
Joost38H 0:eb16ed402ffa 230 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
Joost38H 0:eb16ed402ffa 231 else motor1MagnitudePin = fabs(motorValue1);
Joost38H 0:eb16ed402ffa 232
Joost38H 0:eb16ed402ffa 233 }
Joost38H 0:eb16ed402ffa 234
Joost38H 0:eb16ed402ffa 235 void SetMotor2(double motorValue2)
Joost38H 0:eb16ed402ffa 236 {
Joost38H 0:eb16ed402ffa 237 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 0:eb16ed402ffa 238 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 0:eb16ed402ffa 239 if (motorValue2 >=0) motor2DirectionPin=1;
Joost38H 0:eb16ed402ffa 240 else motor2DirectionPin=0;
Joost38H 0:eb16ed402ffa 241 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
Joost38H 0:eb16ed402ffa 242 else motor2MagnitudePin = fabs(motorValue2);
Joost38H 0:eb16ed402ffa 243
Joost38H 0:eb16ed402ffa 244 }
Joost38H 0:eb16ed402ffa 245
Joost38H 0:eb16ed402ffa 246 double FeedForwardControl1(double referenceVelocity1)
Joost38H 0:eb16ed402ffa 247 {
Joost38H 0:eb16ed402ffa 248 // very simple linear feed-forward control
Joost38H 0:eb16ed402ffa 249 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 0:eb16ed402ffa 250 double motorValue1 = referenceVelocity1 / MotorGain;
Joost38H 0:eb16ed402ffa 251 return motorValue1;
Joost38H 0:eb16ed402ffa 252 }
Joost38H 0:eb16ed402ffa 253
Joost38H 0:eb16ed402ffa 254 double FeedForwardControl2(double referenceVelocity2)
Joost38H 0:eb16ed402ffa 255 {
Joost38H 0:eb16ed402ffa 256 // very simple linear feed-forward control
Joost38H 0:eb16ed402ffa 257 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 0:eb16ed402ffa 258 double motorValue2 = referenceVelocity2 / MotorGain;
Joost38H 0:eb16ed402ffa 259 return motorValue2;
Joost38H 0:eb16ed402ffa 260 }
Joost38H 0:eb16ed402ffa 261
Joost38H 0:eb16ed402ffa 262 void MeasureAndControl1()
Joost38H 0:eb16ed402ffa 263 {
Joost38H 0:eb16ed402ffa 264 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 0:eb16ed402ffa 265 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 0:eb16ed402ffa 266 double referenceVelocity1 = GetReferenceVelocity1();
Joost38H 0:eb16ed402ffa 267 double motorValue1 = FeedForwardControl1(referenceVelocity1);
Joost38H 0:eb16ed402ffa 268 SetMotor1(motorValue1);
Joost38H 0:eb16ed402ffa 269 }
Joost38H 0:eb16ed402ffa 270
Joost38H 0:eb16ed402ffa 271 void MeasureAndControl2()
Joost38H 0:eb16ed402ffa 272 {
Joost38H 0:eb16ed402ffa 273 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 0:eb16ed402ffa 274 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 0:eb16ed402ffa 275 double referenceVelocity2 = GetReferenceVelocity2();
Joost38H 0:eb16ed402ffa 276 double motorValue2 = FeedForwardControl2(referenceVelocity2);
Joost38H 0:eb16ed402ffa 277 SetMotor2(motorValue2);
Joost38H 0:eb16ed402ffa 278 }
Joost38H 0:eb16ed402ffa 279
Joost38H 0:eb16ed402ffa 280 void readdata1()
Joost38H 0:eb16ed402ffa 281 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 0:eb16ed402ffa 282 pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
Joost38H 0:eb16ed402ffa 283 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 0:eb16ed402ffa 284 pc.printf("Delta_M1 = %i \r\n",delta1);
Joost38H 0:eb16ed402ffa 285 }
Joost38H 0:eb16ed402ffa 286
Joost38H 0:eb16ed402ffa 287 void readdata2()
Joost38H 0:eb16ed402ffa 288 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 0:eb16ed402ffa 289 pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
Joost38H 0:eb16ed402ffa 290 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 0:eb16ed402ffa 291 pc.printf("Delta_M2 = %i \r\n",delta2);
Joost38H 0:eb16ed402ffa 292 }
Joost38H 0:eb16ed402ffa 293
Joost38H 0:eb16ed402ffa 294 // einde deel motor------------------------------------------------------------------------------------
Joost38H 0:eb16ed402ffa 295
Joost38H 0:eb16ed402ffa 296 Ticker loop;
Joost38H 0:eb16ed402ffa 297
Joost38H 0:eb16ed402ffa 298 /*Calculates ropelengths that are needed to get to new positions, based on the
Joost38H 0:eb16ed402ffa 299 set coordinates and the position of the poles */
Joost38H 0:eb16ed402ffa 300 double touwlengtes(){
Joost38H 0:eb16ed402ffa 301 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Joost38H 0:eb16ed402ffa 302 Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Joost38H 0:eb16ed402ffa 303 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Joost38H 0:eb16ed402ffa 304 return 0;
Joost38H 0:eb16ed402ffa 305 }
Joost38H 0:eb16ed402ffa 306
Joost38H 0:eb16ed402ffa 307 /* Calculates rotations (and associated counts) of the motor to get to the
Joost38H 0:eb16ed402ffa 308 desired new position*/
Joost38H 0:eb16ed402ffa 309 double turns(){
Joost38H 0:eb16ed402ffa 310
Joost38H 0:eb16ed402ffa 311 roto=Lou/omtrekklosje;
Joost38H 0:eb16ed402ffa 312 rotb=Lbu/omtrekklosje;
Joost38H 0:eb16ed402ffa 313 rotr=Lru/omtrekklosje;
Joost38H 0:eb16ed402ffa 314 counto=roto*4200;
Joost38H 0:eb16ed402ffa 315 //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts
Joost38H 0:eb16ed402ffa 316 countb=rotb*4200;
Joost38H 0:eb16ed402ffa 317 //countb = (int)(countb + 0.5);
Joost38H 0:eb16ed402ffa 318 countr=rotr*4200;
Joost38H 0:eb16ed402ffa 319 //countr = (int)(countr + 0.5);
Joost38H 0:eb16ed402ffa 320 return 0;
Joost38H 0:eb16ed402ffa 321 }
Joost38H 0:eb16ed402ffa 322
Joost38H 0:eb16ed402ffa 323 // Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
Joost38H 0:eb16ed402ffa 324 double Pst(){
Joost38H 0:eb16ed402ffa 325 Pstx=Pex+Vex*T;
Joost38H 0:eb16ed402ffa 326 Psty=Pey+Vey*T;
Joost38H 0:eb16ed402ffa 327 touwlengtes();
Joost38H 0:eb16ed402ffa 328 Pex=Pstx;
Joost38H 0:eb16ed402ffa 329 Pey=Psty;
Joost38H 0:eb16ed402ffa 330 //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
Joost38H 0:eb16ed402ffa 331 //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
Joost38H 0:eb16ed402ffa 332 turns();
Joost38H 0:eb16ed402ffa 333 //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr);
Joost38H 0:eb16ed402ffa 334 pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
Joost38H 0:eb16ed402ffa 335 /*float R;
Joost38H 0:eb16ed402ffa 336 R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt
Joost38H 0:eb16ed402ffa 337 pc.printf("\n\r R=%f",R);*/
Joost38H 0:eb16ed402ffa 338 return 0;
Joost38H 0:eb16ed402ffa 339 }
Joost38H 0:eb16ed402ffa 340
Joost38H 0:eb16ed402ffa 341 //Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
Joost38H 0:eb16ed402ffa 342 double Ps(){
Joost38H 0:eb16ed402ffa 343 Psx=(Xin)*30+121;
Joost38H 0:eb16ed402ffa 344 Psy=(Yin)*30+308;
Joost38H 0:eb16ed402ffa 345 //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
Joost38H 0:eb16ed402ffa 346 return 0;
Joost38H 0:eb16ed402ffa 347 }
Joost38H 0:eb16ed402ffa 348
Joost38H 0:eb16ed402ffa 349 // Rekent dit de snelheid uit waarmee de motoren bewegen?
Joost38H 0:eb16ed402ffa 350 void Ve(){
Joost38H 0:eb16ed402ffa 351 Vex=Kz*(Psx-Pex);
Joost38H 0:eb16ed402ffa 352 Vey=Kz*(Psy-Pey);
Joost38H 0:eb16ed402ffa 353 modVe=sqrt(pow(Vex,2)+pow(Vey,2));
Joost38H 0:eb16ed402ffa 354 if(modVe>Vmax){
Joost38H 0:eb16ed402ffa 355 Vex=(Vex/modVe)*Vmax;
Joost38H 0:eb16ed402ffa 356 Vey=(Vey/modVe)*Vmax;
Joost38H 0:eb16ed402ffa 357 }
Joost38H 0:eb16ed402ffa 358 Pst();
Joost38H 0:eb16ed402ffa 359 pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
Joost38H 0:eb16ed402ffa 360 if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
Joost38H 0:eb16ed402ffa 361 B=true;
Joost38H 0:eb16ed402ffa 362 loop.detach();
Joost38H 0:eb16ed402ffa 363 }
Joost38H 0:eb16ed402ffa 364 }
Joost38H 0:eb16ed402ffa 365
Joost38H 0:eb16ed402ffa 366 // Calculating the desired position, so that the motors can go here
Joost38H 0:eb16ed402ffa 367 int calculator(){
Joost38H 0:eb16ed402ffa 368 Ps();
Joost38H 0:eb16ed402ffa 369 loop.attach(&Ve,0.02);
Joost38H 0:eb16ed402ffa 370 return 0;
Joost38H 0:eb16ed402ffa 371 }
Joost38H 0:eb16ed402ffa 372
Joost38H 0:eb16ed402ffa 373 // Function which makes it possible to lower the end-effector to pick up a piece
Joost38H 0:eb16ed402ffa 374 void zakker(){
Joost38H 0:eb16ed402ffa 375 while(1){
Joost38H 0:eb16ed402ffa 376 wait(1);
Joost38H 0:eb16ed402ffa 377 if(B==true){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is
Joost38H 0:eb16ed402ffa 378 dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
Joost38H 0:eb16ed402ffa 379 dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
Joost38H 0:eb16ed402ffa 380 dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
Joost38H 0:eb16ed402ffa 381 rotzo=dLod/omtrekklosje;
Joost38H 0:eb16ed402ffa 382 rotzb=dLbd/omtrekklosje;
Joost38H 0:eb16ed402ffa 383 rotzr=dLrd/omtrekklosje;
Joost38H 0:eb16ed402ffa 384 countzo=rotzo*4200;
Joost38H 0:eb16ed402ffa 385 countzb=rotzb*4200;
Joost38H 0:eb16ed402ffa 386 countzr=rotzr*4200;
Joost38H 0:eb16ed402ffa 387
Joost38H 0:eb16ed402ffa 388 pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
Joost38H 0:eb16ed402ffa 389 }
Joost38H 0:eb16ed402ffa 390 }
Joost38H 0:eb16ed402ffa 391 }
Joost38H 0:eb16ed402ffa 392
Joost38H 0:eb16ed402ffa 393 int main()
Joost38H 0:eb16ed402ffa 394 {
Joost38H 0:eb16ed402ffa 395 pc.baud(115200);
Joost38H 0:eb16ed402ffa 396 tellerX();
Joost38H 0:eb16ed402ffa 397 tellerY();
Joost38H 0:eb16ed402ffa 398 calculator();
Joost38H 0:eb16ed402ffa 399 controlmotor1.attach(&MeasureAndControl1, 0.01);
Joost38H 0:eb16ed402ffa 400 calculatedelta1.attach(&calcdelta1, 0.01);
Joost38H 0:eb16ed402ffa 401 printdata1.attach(&readdata1, 0.5);
Joost38H 0:eb16ed402ffa 402 controlmotor2.attach(&MeasureAndControl2, 0.01);
Joost38H 0:eb16ed402ffa 403 calculatedelta2.attach(&calcdelta2, 0.01);
Joost38H 0:eb16ed402ffa 404 printdata2.attach(&readdata2, 0.5);
Joost38H 0:eb16ed402ffa 405 //zakker();
Joost38H 0:eb16ed402ffa 406
Joost38H 0:eb16ed402ffa 407 return 0;
Joost38H 0:eb16ed402ffa 408 }