Jitse Giesen
/
Koppeling
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.
main.cpp@4:050dd85ffcf5, 2017-10-23 (annotated)
- Committer:
- Jitse_Giesen
- Date:
- Mon Oct 23 14:29:51 2017 +0000
- Revision:
- 4:050dd85ffcf5
- Parent:
- 3:08bf605844eb
- Child:
- 5:aa191ca932ab
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Jitse_Giesen | 0:3e98322ac0e2 | 1 | #include "mbed.h" |
| Jitse_Giesen | 0:3e98322ac0e2 | 2 | #include "math.h" |
| Jitse_Giesen | 4:050dd85ffcf5 | 3 | #include "encoder.h" |
| Jitse_Giesen | 4:050dd85ffcf5 | 4 | #include "QEI.h" |
| Jitse_Giesen | 0:3e98322ac0e2 | 5 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 6 | DigitalIn button1(D8); |
| Jitse_Giesen | 4:050dd85ffcf5 | 7 | DigitalIn button2(D9); |
| Jitse_Giesen | 4:050dd85ffcf5 | 8 | //AnalogIn potMeterIn(A1); |
| Jitse_Giesen | 4:050dd85ffcf5 | 9 | float potMeterIn; // = 0.01; snelheid in, zonder potmeter gebruik |
| Jitse_Giesen | 4:050dd85ffcf5 | 10 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 11 | DigitalOut motor1DirectionPin(D4); |
| Jitse_Giesen | 4:050dd85ffcf5 | 12 | PwmOut motor1MagnitudePin(D5); |
| Jitse_Giesen | 4:050dd85ffcf5 | 13 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 14 | QEI Encoder(D12,D13,NC,32); |
| Jitse_Giesen | 4:050dd85ffcf5 | 15 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 16 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 17 | int counts = Encoder.getPulses(); |
| Jitse_Giesen | 4:050dd85ffcf5 | 18 | int delta; |
| Jitse_Giesen | 0:3e98322ac0e2 | 19 | DigitalOut led(LED_RED); |
| Jitse_Giesen | 0:3e98322ac0e2 | 20 | DigitalOut ledd(LED_GREEN); |
| Jitse_Giesen | 2:263a53d72bb7 | 21 | DigitalOut leddd(LED_BLUE); |
| Jitse_Giesen | 0:3e98322ac0e2 | 22 | InterruptIn button(SW2); |
| Jitse_Giesen | 0:3e98322ac0e2 | 23 | Timer t; |
| Jitse_Giesen | 0:3e98322ac0e2 | 24 | Serial pc(USBTX, USBRX); |
| Jitse_Giesen | 0:3e98322ac0e2 | 25 | int Xin=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 26 | float huidigetijdX; |
| Jitse_Giesen | 0:3e98322ac0e2 | 27 | void ledtX(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 28 | t.reset(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 29 | Xin++; |
| Jitse_Giesen | 0:3e98322ac0e2 | 30 | pc.printf("Xin is %i\n",Xin); |
| Jitse_Giesen | 0:3e98322ac0e2 | 31 | ledd=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 32 | led=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 33 | wait(0.5); |
| Jitse_Giesen | 0:3e98322ac0e2 | 34 | ledd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 35 | led=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 36 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 37 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 38 | int tellerX(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 39 | ledd=1; |
| Jitse_Giesen | 2:263a53d72bb7 | 40 | leddd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 41 | pc.baud(115200); |
| Jitse_Giesen | 0:3e98322ac0e2 | 42 | while(true){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 43 | button.fall(ledtX); |
| Jitse_Giesen | 0:3e98322ac0e2 | 44 | /*if (EMG>threshold){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 45 | Piek(); // dit is wat je uiteindelijk wil dat er staat |
| Jitse_Giesen | 0:3e98322ac0e2 | 46 | }*/ |
| Jitse_Giesen | 0:3e98322ac0e2 | 47 | t.start(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 48 | huidigetijdX=t.read(); |
| Jitse_Giesen | 3:08bf605844eb | 49 | if (huidigetijdX>2){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 50 | led=1; // ga door naar het volgende programma |
| Jitse_Giesen | 2:263a53d72bb7 | 51 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 52 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 53 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 54 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 55 | int Yin=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 56 | float huidigetijdY; |
| Jitse_Giesen | 0:3e98322ac0e2 | 57 | void ledtY(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 58 | t.reset(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 59 | Yin++; |
| Jitse_Giesen | 0:3e98322ac0e2 | 60 | pc.printf("Yin is %i\n",Yin); |
| Jitse_Giesen | 0:3e98322ac0e2 | 61 | ledd=0; |
| Jitse_Giesen | 2:263a53d72bb7 | 62 | leddd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 63 | wait(0.5); |
| Jitse_Giesen | 0:3e98322ac0e2 | 64 | ledd=1; |
| Jitse_Giesen | 2:263a53d72bb7 | 65 | leddd=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 66 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 67 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 68 | int tellerY(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 69 | t.reset(); |
| Jitse_Giesen | 2:263a53d72bb7 | 70 | ledd=1; |
| Jitse_Giesen | 2:263a53d72bb7 | 71 | leddd=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 72 | pc.baud(115200); |
| Jitse_Giesen | 0:3e98322ac0e2 | 73 | while(true){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 74 | button.fall(ledtY); |
| Jitse_Giesen | 0:3e98322ac0e2 | 75 | /*if (EMG>threshold){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 76 | Piek(); // dit is wat je uiteindelijk wil dat er staat |
| Jitse_Giesen | 0:3e98322ac0e2 | 77 | }*/ |
| Jitse_Giesen | 0:3e98322ac0e2 | 78 | t.start(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 79 | huidigetijdY=t.read(); |
| Jitse_Giesen | 3:08bf605844eb | 80 | if (huidigetijdY>2){ |
| Jitse_Giesen | 2:263a53d72bb7 | 81 | leddd=1; |
| Jitse_Giesen | 2:263a53d72bb7 | 82 | button.fall(0); |
| Jitse_Giesen | 3:08bf605844eb | 83 | return 0; // ga door naar het volgende programma |
| Jitse_Giesen | 0:3e98322ac0e2 | 84 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 85 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 86 | } |
| Jitse_Giesen | 2:263a53d72bb7 | 87 | int B; |
| Jitse_Giesen | 0:3e98322ac0e2 | 88 | float Pox = 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 89 | float Poy = 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 90 | float Pbx = 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 91 | float Pby = 887; |
| Jitse_Giesen | 0:3e98322ac0e2 | 92 | float Prx = 768; |
| Jitse_Giesen | 0:3e98322ac0e2 | 93 | float Pry = 443; |
| Jitse_Giesen | 0:3e98322ac0e2 | 94 | float Pex=121; |
| Jitse_Giesen | 0:3e98322ac0e2 | 95 | float Pey=308; |
| Jitse_Giesen | 0:3e98322ac0e2 | 96 | float diamtrklosje=20; |
| Jitse_Giesen | 0:3e98322ac0e2 | 97 | float pi=3.14159265359; |
| Jitse_Giesen | 2:263a53d72bb7 | 98 | float omtrekklosje=diamtrklosje*pi; |
| Jitse_Giesen | 0:3e98322ac0e2 | 99 | float Lou; |
| Jitse_Giesen | 0:3e98322ac0e2 | 100 | float Lbu; |
| Jitse_Giesen | 0:3e98322ac0e2 | 101 | float Lru; |
| Jitse_Giesen | 2:263a53d72bb7 | 102 | float dLod; |
| Jitse_Giesen | 2:263a53d72bb7 | 103 | float dLbd; |
| Jitse_Giesen | 2:263a53d72bb7 | 104 | float dLrd; |
| Jitse_Giesen | 0:3e98322ac0e2 | 105 | float roto; |
| Jitse_Giesen | 0:3e98322ac0e2 | 106 | float rotb; |
| Jitse_Giesen | 0:3e98322ac0e2 | 107 | float rotr; |
| Jitse_Giesen | 2:263a53d72bb7 | 108 | float rotzo; |
| Jitse_Giesen | 2:263a53d72bb7 | 109 | float rotzb; |
| Jitse_Giesen | 2:263a53d72bb7 | 110 | float rotzr; |
| Jitse_Giesen | 0:3e98322ac0e2 | 111 | float counto; |
| Jitse_Giesen | 0:3e98322ac0e2 | 112 | float countb; |
| Jitse_Giesen | 2:263a53d72bb7 | 113 | float countr; |
| Jitse_Giesen | 2:263a53d72bb7 | 114 | float countzo; |
| Jitse_Giesen | 2:263a53d72bb7 | 115 | float countzb; |
| Jitse_Giesen | 2:263a53d72bb7 | 116 | float countzr; |
| Jitse_Giesen | 0:3e98322ac0e2 | 117 | float Psx; |
| Jitse_Giesen | 0:3e98322ac0e2 | 118 | float Psy; |
| Jitse_Giesen | 0:3e98322ac0e2 | 119 | float Vex; |
| Jitse_Giesen | 0:3e98322ac0e2 | 120 | float Vey; |
| Jitse_Giesen | 4:050dd85ffcf5 | 121 | float Kz=0.7; // nadersnelheid instellen |
| Jitse_Giesen | 0:3e98322ac0e2 | 122 | float modVe; |
| Jitse_Giesen | 0:3e98322ac0e2 | 123 | float Vmax=20; |
| Jitse_Giesen | 0:3e98322ac0e2 | 124 | float Pstx; |
| Jitse_Giesen | 0:3e98322ac0e2 | 125 | float Psty; |
| Jitse_Giesen | 0:3e98322ac0e2 | 126 | float T=0.02;//seconds |
| Jitse_Giesen | 0:3e98322ac0e2 | 127 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 128 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 129 | //Deel om motor(en) aan te sturen-------------------------------------------- |
| Jitse_Giesen | 4:050dd85ffcf5 | 130 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 131 | void calcdelta(void) { |
| Jitse_Giesen | 4:050dd85ffcf5 | 132 | delta = (counto - Encoder.getPulses()); |
| Jitse_Giesen | 4:050dd85ffcf5 | 133 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 134 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 135 | void FeedBack(void) |
| Jitse_Giesen | 4:050dd85ffcf5 | 136 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 137 | if (delta > 100) |
| Jitse_Giesen | 4:050dd85ffcf5 | 138 | { potMeterIn = -0.1; |
| Jitse_Giesen | 4:050dd85ffcf5 | 139 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 140 | if (delta < -100) |
| Jitse_Giesen | 4:050dd85ffcf5 | 141 | { potMeterIn = 0.1; |
| Jitse_Giesen | 4:050dd85ffcf5 | 142 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 143 | else |
| Jitse_Giesen | 4:050dd85ffcf5 | 144 | { potMeterIn = 0; |
| Jitse_Giesen | 4:050dd85ffcf5 | 145 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 146 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 147 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 148 | //Encoder motor1(D12,D13); //Gear ratio = encoder ratio = 131.25:1 & |
| Jitse_Giesen | 4:050dd85ffcf5 | 149 | // factor 64: To compute the counts per revolution of the gearbox output, multiply the gear ratio by 64. |
| Jitse_Giesen | 4:050dd85ffcf5 | 150 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 151 | float referenceVelocity; |
| Jitse_Giesen | 4:050dd85ffcf5 | 152 | float motorValue; |
| Jitse_Giesen | 4:050dd85ffcf5 | 153 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 154 | Ticker controlmotor1; // één ticker van maken? |
| Jitse_Giesen | 4:050dd85ffcf5 | 155 | Ticker calculatedelta; |
| Jitse_Giesen | 4:050dd85ffcf5 | 156 | Ticker feedbacktick; |
| Jitse_Giesen | 4:050dd85ffcf5 | 157 | Ticker printdata; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes |
| Jitse_Giesen | 4:050dd85ffcf5 | 158 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 159 | float GetReferenceVelocity() |
| Jitse_Giesen | 4:050dd85ffcf5 | 160 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 161 | // Returns reference velocity in rad/s. |
| Jitse_Giesen | 4:050dd85ffcf5 | 162 | // Positive value means clockwise rotation. |
| Jitse_Giesen | 4:050dd85ffcf5 | 163 | float maxVelocity=Vex*25+Vey*25; // max 8.4 in rad/s of course! |
| Jitse_Giesen | 4:050dd85ffcf5 | 164 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 165 | if (button1) |
| Jitse_Giesen | 4:050dd85ffcf5 | 166 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 167 | // Clockwise rotation |
| Jitse_Giesen | 4:050dd85ffcf5 | 168 | referenceVelocity = -1*potMeterIn * maxVelocity; |
| Jitse_Giesen | 4:050dd85ffcf5 | 169 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 170 | else |
| Jitse_Giesen | 4:050dd85ffcf5 | 171 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 172 | // Counterclockwise rotation |
| Jitse_Giesen | 4:050dd85ffcf5 | 173 | referenceVelocity = potMeterIn * maxVelocity; |
| Jitse_Giesen | 4:050dd85ffcf5 | 174 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 175 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 176 | if (button2) |
| Jitse_Giesen | 4:050dd85ffcf5 | 177 | { potMeterIn = potMeterIn*1; |
| Jitse_Giesen | 4:050dd85ffcf5 | 178 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 179 | else |
| Jitse_Giesen | 4:050dd85ffcf5 | 180 | { potMeterIn = potMeterIn*0; |
| Jitse_Giesen | 4:050dd85ffcf5 | 181 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 182 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 183 | if (Encoder.getPulses() < counto) |
| Jitse_Giesen | 4:050dd85ffcf5 | 184 | { potMeterIn = 1; |
| Jitse_Giesen | 4:050dd85ffcf5 | 185 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 186 | else |
| Jitse_Giesen | 4:050dd85ffcf5 | 187 | { potMeterIn = 0; |
| Jitse_Giesen | 4:050dd85ffcf5 | 188 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 189 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 190 | return referenceVelocity; |
| Jitse_Giesen | 4:050dd85ffcf5 | 191 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 192 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 193 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 194 | void SetMotor1(float motorValue) |
| Jitse_Giesen | 4:050dd85ffcf5 | 195 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 196 | // Given -1<=motorValue<=1, this sets the PWM and direction |
| Jitse_Giesen | 4:050dd85ffcf5 | 197 | // bits for motor 1. Positive value makes motor rotating |
| Jitse_Giesen | 4:050dd85ffcf5 | 198 | // clockwise. motorValues outside range are truncated to |
| Jitse_Giesen | 4:050dd85ffcf5 | 199 | // within range |
| Jitse_Giesen | 4:050dd85ffcf5 | 200 | if (motorValue >=0) motor1DirectionPin=1; |
| Jitse_Giesen | 4:050dd85ffcf5 | 201 | else motor1DirectionPin=0; |
| Jitse_Giesen | 4:050dd85ffcf5 | 202 | if (fabs(motorValue)>1) motor1MagnitudePin = 1; |
| Jitse_Giesen | 4:050dd85ffcf5 | 203 | else motor1MagnitudePin = fabs(motorValue); |
| Jitse_Giesen | 4:050dd85ffcf5 | 204 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 205 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 206 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 207 | float FeedForwardControl(float referenceVelocity) |
| Jitse_Giesen | 4:050dd85ffcf5 | 208 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 209 | // very simple linear feed-forward control |
| Jitse_Giesen | 4:050dd85ffcf5 | 210 | const float MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 |
| Jitse_Giesen | 4:050dd85ffcf5 | 211 | float motorValue = referenceVelocity / MotorGain; |
| Jitse_Giesen | 4:050dd85ffcf5 | 212 | return motorValue; |
| Jitse_Giesen | 4:050dd85ffcf5 | 213 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 214 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 215 | void MeasureAndControl(void) |
| Jitse_Giesen | 4:050dd85ffcf5 | 216 | { |
| Jitse_Giesen | 4:050dd85ffcf5 | 217 | // This function measures the potmeter position, extracts a |
| Jitse_Giesen | 4:050dd85ffcf5 | 218 | // reference velocity from it, and controls the motor with |
| Jitse_Giesen | 4:050dd85ffcf5 | 219 | // a simple FeedForward controller. Call this from a Ticker. |
| Jitse_Giesen | 4:050dd85ffcf5 | 220 | float referenceVelocity = GetReferenceVelocity(); |
| Jitse_Giesen | 4:050dd85ffcf5 | 221 | float motorValue = FeedForwardControl(referenceVelocity); |
| Jitse_Giesen | 4:050dd85ffcf5 | 222 | SetMotor1(motorValue); |
| Jitse_Giesen | 4:050dd85ffcf5 | 223 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 224 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 225 | void readdata(void) |
| Jitse_Giesen | 4:050dd85ffcf5 | 226 | { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); |
| Jitse_Giesen | 4:050dd85ffcf5 | 227 | pc.printf("Pulses = %i \r\n",Encoder.getPulses()); |
| Jitse_Giesen | 4:050dd85ffcf5 | 228 | //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); |
| Jitse_Giesen | 4:050dd85ffcf5 | 229 | pc.printf("Delta = %i \r\n",delta); |
| Jitse_Giesen | 4:050dd85ffcf5 | 230 | } |
| Jitse_Giesen | 4:050dd85ffcf5 | 231 | |
| Jitse_Giesen | 4:050dd85ffcf5 | 232 | // einde deel motor------------------------------------------------------------------------------------ |
| Jitse_Giesen | 4:050dd85ffcf5 | 233 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 234 | Ticker loop; |
| Jitse_Giesen | 0:3e98322ac0e2 | 235 | float touwlengtes(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 236 | Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2)); // rekent touwlengtes uit die nodig zijn voor de nieuwe positie aan de hand van de ingegeven coördinaten en de posities van de palen |
| Jitse_Giesen | 0:3e98322ac0e2 | 237 | Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2)); |
| Jitse_Giesen | 0:3e98322ac0e2 | 238 | Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2)); |
| Jitse_Giesen | 0:3e98322ac0e2 | 239 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 240 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 241 | float turns(){ |
| Jitse_Giesen | 2:263a53d72bb7 | 242 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 243 | roto=Lou/omtrekklosje; |
| Jitse_Giesen | 0:3e98322ac0e2 | 244 | rotb=Lbu/omtrekklosje; |
| Jitse_Giesen | 0:3e98322ac0e2 | 245 | rotr=Lru/omtrekklosje; |
| Jitse_Giesen | 4:050dd85ffcf5 | 246 | counto=roto*4200; |
| Jitse_Giesen | 4:050dd85ffcf5 | 247 | //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts |
| Jitse_Giesen | 0:3e98322ac0e2 | 248 | countb=rotb*4200; |
| Jitse_Giesen | 4:050dd85ffcf5 | 249 | //countb = (int)(countb + 0.5); |
| Jitse_Giesen | 0:3e98322ac0e2 | 250 | countr=rotr*4200; |
| Jitse_Giesen | 4:050dd85ffcf5 | 251 | //countr = (int)(countr + 0.5); |
| Jitse_Giesen | 0:3e98322ac0e2 | 252 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 253 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 254 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 255 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 256 | float Pst(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 257 | Pstx=Pex+Vex*T; |
| Jitse_Giesen | 0:3e98322ac0e2 | 258 | Psty=Pey+Vey*T; |
| Jitse_Giesen | 0:3e98322ac0e2 | 259 | touwlengtes(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 260 | Pex=Pstx; //Dit is nog een aanname. Moet zijn: read the encoder and put the real position as the Pe for the next step |
| Jitse_Giesen | 0:3e98322ac0e2 | 261 | Pey=Psty; |
| Jitse_Giesen | 0:3e98322ac0e2 | 262 | //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty); |
| Jitse_Giesen | 0:3e98322ac0e2 | 263 | //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru); |
| Jitse_Giesen | 0:3e98322ac0e2 | 264 | turns(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 265 | //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); |
| Jitse_Giesen | 0:3e98322ac0e2 | 266 | pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr); |
| Jitse_Giesen | 0:3e98322ac0e2 | 267 | /*float R; |
| Jitse_Giesen | 0:3e98322ac0e2 | 268 | R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constand is en de end efector dus een rechte lijn maakt |
| Jitse_Giesen | 0:3e98322ac0e2 | 269 | pc.printf("\n\r R=%f",R);*/ |
| Jitse_Giesen | 0:3e98322ac0e2 | 270 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 271 | } |
| Jitse_Giesen | 2:263a53d72bb7 | 272 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 273 | float Ps(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 274 | Psx=(Xin)*30+121; |
| Jitse_Giesen | 0:3e98322ac0e2 | 275 | Psy=(Yin)*30+308; |
| Jitse_Giesen | 0:3e98322ac0e2 | 276 | pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); |
| Jitse_Giesen | 0:3e98322ac0e2 | 277 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 278 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 279 | void Ve(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 280 | Vex=Kz*(Psx-Pex); |
| Jitse_Giesen | 0:3e98322ac0e2 | 281 | Vey=Kz*(Psy-Pey); |
| Jitse_Giesen | 0:3e98322ac0e2 | 282 | modVe=sqrt(pow(Vex,2)+pow(Vey,2)); |
| Jitse_Giesen | 0:3e98322ac0e2 | 283 | if(modVe>Vmax){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 284 | Vex=(Vex/modVe)*Vmax; |
| Jitse_Giesen | 0:3e98322ac0e2 | 285 | Vey=(Vey/modVe)*Vmax; |
| Jitse_Giesen | 0:3e98322ac0e2 | 286 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 287 | Pst(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 288 | pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); |
| Jitse_Giesen | 0:3e98322ac0e2 | 289 | if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ |
| Jitse_Giesen | 2:263a53d72bb7 | 290 | B=5; |
| Jitse_Giesen | 2:263a53d72bb7 | 291 | loop.detach(); |
| Jitse_Giesen | 2:263a53d72bb7 | 292 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 293 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 294 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 295 | int calculator(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 296 | pc.baud(115200); |
| Jitse_Giesen | 0:3e98322ac0e2 | 297 | Ps(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 298 | loop.attach(&Ve,0.02); |
| Jitse_Giesen | 0:3e98322ac0e2 | 299 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 300 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 301 | |
| Jitse_Giesen | 2:263a53d72bb7 | 302 | void zakker(){ |
| Jitse_Giesen | 2:263a53d72bb7 | 303 | while(1){ |
| Jitse_Giesen | 2:263a53d72bb7 | 304 | wait(1); |
| Jitse_Giesen | 4:050dd85ffcf5 | 305 | if(B==5){ // hierdoor wacht dit programma totdat de beweging klaar is |
| Jitse_Giesen | 4:050dd85ffcf5 | 306 | dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken |
| Jitse_Giesen | 2:263a53d72bb7 | 307 | dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu; |
| Jitse_Giesen | 2:263a53d72bb7 | 308 | dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru; |
| Jitse_Giesen | 2:263a53d72bb7 | 309 | rotzo=dLod/omtrekklosje; |
| Jitse_Giesen | 2:263a53d72bb7 | 310 | rotzb=dLbd/omtrekklosje; |
| Jitse_Giesen | 2:263a53d72bb7 | 311 | rotzr=dLrd/omtrekklosje; |
| Jitse_Giesen | 2:263a53d72bb7 | 312 | countzo=rotzo*4200; |
| Jitse_Giesen | 2:263a53d72bb7 | 313 | countzb=rotzb*4200; |
| Jitse_Giesen | 2:263a53d72bb7 | 314 | countzr=rotzr*4200; |
| Jitse_Giesen | 2:263a53d72bb7 | 315 | |
| Jitse_Giesen | 2:263a53d72bb7 | 316 | pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat |
| Jitse_Giesen | 2:263a53d72bb7 | 317 | } |
| Jitse_Giesen | 2:263a53d72bb7 | 318 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 319 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 320 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 321 | int main() |
| Jitse_Giesen | 0:3e98322ac0e2 | 322 | { |
| Jitse_Giesen | 0:3e98322ac0e2 | 323 | tellerX(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 324 | tellerY(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 325 | calculator(); |
| Jitse_Giesen | 4:050dd85ffcf5 | 326 | controlmotor1.attach(&MeasureAndControl, 0.01); |
| Jitse_Giesen | 4:050dd85ffcf5 | 327 | calculatedelta.attach(&calcdelta, 0.01); |
| Jitse_Giesen | 4:050dd85ffcf5 | 328 | if (abs(delta) < -50) // dit moet denk ik in een ticker staan dus bij calcdelta in denk ik, moeten we maar ff proberen!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
| Jitse_Giesen | 4:050dd85ffcf5 | 329 | { feedbacktick.attach(&FeedBack, 0.01);} |
| Jitse_Giesen | 4:050dd85ffcf5 | 330 | printdata.attach(&readdata, 0.5); |
| Jitse_Giesen | 4:050dd85ffcf5 | 331 | //zakker(); |
| Jitse_Giesen | 4:050dd85ffcf5 | 332 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 333 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 334 | } |