Alle functies van de motoren werken
Dependencies: MODSERIAL QEI mbed
Fork of worknotjet by
main.cpp@7:c7bdeee1bbac, 2016-10-21 (annotated)
- Committer:
- Frostworks
- Date:
- Fri Oct 21 10:28:27 2016 +0000
- Revision:
- 7:c7bdeee1bbac
- Parent:
- 6:4e4df2f6157e
- Child:
- 8:8a3ab396853f
motor 3 roteert niet terug en te hard
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Frostworks | 0:2b420376e01d | 1 | #include "mbed.h" |
Frostworks | 0:2b420376e01d | 2 | #include "MODSERIAL.h" |
Frostworks | 6:4e4df2f6157e | 3 | #include "QEI.h" |
Frostworks | 0:2b420376e01d | 4 | |
Frostworks | 6:4e4df2f6157e | 5 | |
Frostworks | 6:4e4df2f6157e | 6 | DigitalOut M1_Rotate(D2); // voltage only base rotation |
Frostworks | 1:833c73834749 | 7 | PwmOut M1_Speed(D3); // voltage only base rotation |
Frostworks | 1:833c73834749 | 8 | |
Frostworks | 6:4e4df2f6157e | 9 | MODSERIAL pc(USBTX, USBRX); |
Frostworks | 6:4e4df2f6157e | 10 | |
Frostworks | 6:4e4df2f6157e | 11 | //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING) |
Frostworks | 6:4e4df2f6157e | 12 | QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING); |
Frostworks | 6:4e4df2f6157e | 13 | QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING); |
Frostworks | 6:4e4df2f6157e | 14 | |
Frostworks | 6:4e4df2f6157e | 15 | DigitalOut M2_Rotate(D4); // encoder side pot 2 translation |
Frostworks | 1:833c73834749 | 16 | PwmOut M2_Speed(D5); // encoder side pot 2 translation |
Frostworks | 1:833c73834749 | 17 | |
Frostworks | 6:4e4df2f6157e | 18 | DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation |
Frostworks | 1:833c73834749 | 19 | PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation |
Frostworks | 0:2b420376e01d | 20 | |
Frostworks | 2:4fb733fffd1f | 21 | DigitalIn links(SW3); |
Frostworks | 2:4fb733fffd1f | 22 | DigitalIn rechts(SW2); |
Frostworks | 0:2b420376e01d | 23 | //DigitalOut M2_rotate(D6); |
Frostworks | 0:2b420376e01d | 24 | //PwmOut M2_Speed(D7); |
Frostworks | 0:2b420376e01d | 25 | |
Frostworks | 1:833c73834749 | 26 | AnalogIn pot1(A4); // pot 1 motor 1 |
Frostworks | 1:833c73834749 | 27 | AnalogIn pot2(A3); // pot 2 motor 3 |
Frostworks | 1:833c73834749 | 28 | |
Frostworks | 3:dfb4b8d37bab | 29 | bool draairechts; |
Frostworks | 3:dfb4b8d37bab | 30 | bool draailinks; |
Frostworks | 6:4e4df2f6157e | 31 | bool turn = 0; |
Frostworks | 5:353ed56417a2 | 32 | float waiter = 0.1; |
Frostworks | 7:c7bdeee1bbac | 33 | float afstand = 50; |
Frostworks | 6:4e4df2f6157e | 34 | float translation = 0; |
Frostworks | 7:c7bdeee1bbac | 35 | float degrees3 = 0; |
Frostworks | 7:c7bdeee1bbac | 36 | |
Frostworks | 7:c7bdeee1bbac | 37 | float Puls_degree = (8400/360); |
Frostworks | 7:c7bdeee1bbac | 38 | float wheel1 = 16; |
Frostworks | 7:c7bdeee1bbac | 39 | float wheel2 = 31; |
Frostworks | 7:c7bdeee1bbac | 40 | float wheel3 = 41; |
Frostworks | 7:c7bdeee1bbac | 41 | float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1)); |
Frostworks | 7:c7bdeee1bbac | 42 | float pi = 3.14159265359; |
Frostworks | 7:c7bdeee1bbac | 43 | |
Frostworks | 3:dfb4b8d37bab | 44 | void GetDirections() |
Frostworks | 3:dfb4b8d37bab | 45 | { |
Frostworks | 5:353ed56417a2 | 46 | pc.baud(115200); |
Frostworks | 6:4e4df2f6157e | 47 | if ((rechts == 0) && (links == 0) && (turn == 0)) { |
Frostworks | 5:353ed56417a2 | 48 | draailinks = 0; |
Frostworks | 5:353ed56417a2 | 49 | draairechts = 0; |
Frostworks | 6:4e4df2f6157e | 50 | turn = 1; |
Frostworks | 5:353ed56417a2 | 51 | pc.printf("begin de actie \n \r "); |
Frostworks | 5:353ed56417a2 | 52 | wait(waiter); |
Frostworks | 5:353ed56417a2 | 53 | |
Frostworks | 6:4e4df2f6157e | 54 | } else if ((rechts == 0) && (links == 0) && (turn == 1)) { |
Frostworks | 5:353ed56417a2 | 55 | draailinks = 0; |
Frostworks | 5:353ed56417a2 | 56 | draairechts = 0; |
Frostworks | 6:4e4df2f6157e | 57 | turn = 0; |
Frostworks | 5:353ed56417a2 | 58 | pc.printf("breek de actie af \n \r "); |
Frostworks | 5:353ed56417a2 | 59 | wait(waiter); |
Frostworks | 6:4e4df2f6157e | 60 | } else if ((rechts == 1) && (links == 1)&& (turn == 0)) { |
Frostworks | 5:353ed56417a2 | 61 | |
Frostworks | 6:4e4df2f6157e | 62 | } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) { |
Frostworks | 5:353ed56417a2 | 63 | /* if the right button is pressed and the motor isn't rotating to the left, |
Frostworks | 5:353ed56417a2 | 64 | then start rotating to the right etc*/ |
Frostworks | 3:dfb4b8d37bab | 65 | draairechts = !draairechts; |
Frostworks | 5:353ed56417a2 | 66 | pc.printf("draai naar rechts \n \r "); |
Frostworks | 5:353ed56417a2 | 67 | wait(waiter); |
Frostworks | 6:4e4df2f6157e | 68 | } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) { |
Frostworks | 3:dfb4b8d37bab | 69 | draailinks = 0; |
Frostworks | 4:7b8bf798bb72 | 70 | draairechts = !draairechts; |
Frostworks | 5:353ed56417a2 | 71 | pc.printf("draai naar rechts na links \n \r "); |
Frostworks | 5:353ed56417a2 | 72 | wait(waiter); |
Frostworks | 6:4e4df2f6157e | 73 | } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) { |
Frostworks | 4:7b8bf798bb72 | 74 | draailinks = !draailinks; |
Frostworks | 5:353ed56417a2 | 75 | pc.printf("draai naar links \n \r "); |
Frostworks | 5:353ed56417a2 | 76 | wait(waiter); |
Frostworks | 6:4e4df2f6157e | 77 | } else if ((links == 1) && (draairechts == 1) && (turn == 0)) { |
Frostworks | 3:dfb4b8d37bab | 78 | draairechts = 0; |
Frostworks | 3:dfb4b8d37bab | 79 | draailinks = !draailinks; |
Frostworks | 5:353ed56417a2 | 80 | pc.printf("draai naar links na rechts \n \r "); |
Frostworks | 5:353ed56417a2 | 81 | wait(waiter); |
Frostworks | 3:dfb4b8d37bab | 82 | } |
Frostworks | 5:353ed56417a2 | 83 | wait(2*waiter); |
Frostworks | 3:dfb4b8d37bab | 84 | } |
Frostworks | 6:4e4df2f6157e | 85 | |
Frostworks | 7:c7bdeee1bbac | 86 | float GetPositionM2() |
Frostworks | 7:c7bdeee1bbac | 87 | { |
Frostworks | 7:c7bdeee1bbac | 88 | float pulses2 = motor2.getPulses(); |
Frostworks | 7:c7bdeee1bbac | 89 | float degrees2 = (pulses2/Puls_degree); |
Frostworks | 7:c7bdeee1bbac | 90 | float radians2 = (degrees2/360)*2*pi; |
Frostworks | 7:c7bdeee1bbac | 91 | float translation = ((radians2/overbrenging)*32.25); |
Frostworks | 7:c7bdeee1bbac | 92 | |
Frostworks | 7:c7bdeee1bbac | 93 | return translation; |
Frostworks | 7:c7bdeee1bbac | 94 | } |
Frostworks | 7:c7bdeee1bbac | 95 | float GetRotationM3() |
Frostworks | 6:4e4df2f6157e | 96 | { |
Frostworks | 7:c7bdeee1bbac | 97 | float pulses3 = motor3.getPulses(); |
Frostworks | 7:c7bdeee1bbac | 98 | float degrees3 = (pulses3/Puls_degree); |
Frostworks | 7:c7bdeee1bbac | 99 | float radians3 = (degrees3/360)*2*pi; |
Frostworks | 7:c7bdeee1bbac | 100 | |
Frostworks | 7:c7bdeee1bbac | 101 | return degrees3; |
Frostworks | 7:c7bdeee1bbac | 102 | } |
Frostworks | 7:c7bdeee1bbac | 103 | void GoBack() |
Frostworks | 7:c7bdeee1bbac | 104 | { |
Frostworks | 7:c7bdeee1bbac | 105 | while (GetPositionM2() > 0) { |
Frostworks | 7:c7bdeee1bbac | 106 | |
Frostworks | 7:c7bdeee1bbac | 107 | M2_Speed = 0.8; |
Frostworks | 7:c7bdeee1bbac | 108 | M2_Rotate = 0; |
Frostworks | 7:c7bdeee1bbac | 109 | pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); |
Frostworks | 7:c7bdeee1bbac | 110 | } |
Frostworks | 7:c7bdeee1bbac | 111 | M2_Speed = 0; |
Frostworks | 6:4e4df2f6157e | 112 | |
Frostworks | 6:4e4df2f6157e | 113 | |
Frostworks | 7:c7bdeee1bbac | 114 | while (GetRotationM3 < 0) { |
Frostworks | 7:c7bdeee1bbac | 115 | M3_Rotate = 1; |
Frostworks | 7:c7bdeee1bbac | 116 | M3_Speed = 0.2; |
Frostworks | 7:c7bdeee1bbac | 117 | |
Frostworks | 7:c7bdeee1bbac | 118 | } |
Frostworks | 7:c7bdeee1bbac | 119 | M3_Speed = 0; |
Frostworks | 7:c7bdeee1bbac | 120 | |
Frostworks | 7:c7bdeee1bbac | 121 | turn = 0; |
Frostworks | 7:c7bdeee1bbac | 122 | |
Frostworks | 6:4e4df2f6157e | 123 | } |
Frostworks | 6:4e4df2f6157e | 124 | void Burgerflip() |
Frostworks | 6:4e4df2f6157e | 125 | { |
Frostworks | 7:c7bdeee1bbac | 126 | if (GetPositionM2() > afstand) { |
Frostworks | 7:c7bdeee1bbac | 127 | M3_Speed = 0.2; |
Frostworks | 7:c7bdeee1bbac | 128 | M3_Rotate = 0; |
Frostworks | 7:c7bdeee1bbac | 129 | M2_Speed = 0; |
Frostworks | 7:c7bdeee1bbac | 130 | } else if (GetPositionM2() < afstand) { |
Frostworks | 7:c7bdeee1bbac | 131 | M2_Speed = 0.8; |
Frostworks | 7:c7bdeee1bbac | 132 | M2_Rotate = 1; |
Frostworks | 7:c7bdeee1bbac | 133 | } |
Frostworks | 7:c7bdeee1bbac | 134 | if (GetRotationM3() > 75) { |
Frostworks | 7:c7bdeee1bbac | 135 | GoBack(); |
Frostworks | 7:c7bdeee1bbac | 136 | } |
Frostworks | 7:c7bdeee1bbac | 137 | } |
Frostworks | 6:4e4df2f6157e | 138 | |
Frostworks | 0:2b420376e01d | 139 | int main() |
Frostworks | 0:2b420376e01d | 140 | { |
Frostworks | 6:4e4df2f6157e | 141 | while (true) { |
Frostworks | 7:c7bdeee1bbac | 142 | |
Frostworks | 6:4e4df2f6157e | 143 | GetDirections(); |
Frostworks | 6:4e4df2f6157e | 144 | if (draairechts == true) { |
Frostworks | 7:c7bdeee1bbac | 145 | M1_Speed = 0.2; |
Frostworks | 6:4e4df2f6157e | 146 | M1_Rotate = 0; |
Frostworks | 6:4e4df2f6157e | 147 | } else if (draailinks == true) { |
Frostworks | 7:c7bdeee1bbac | 148 | M1_Speed = 0.2; |
Frostworks | 6:4e4df2f6157e | 149 | M1_Rotate = 1; |
Frostworks | 6:4e4df2f6157e | 150 | } else if (turn == 1) { |
Frostworks | 6:4e4df2f6157e | 151 | /*M2_Speed = 0.5; |
Frostworks | 6:4e4df2f6157e | 152 | M2_Rotate = 1; |
Frostworks | 6:4e4df2f6157e | 153 | M3_Speed = 0.5; |
Frostworks | 6:4e4df2f6157e | 154 | M3_Rotate = 1;*/ |
Frostworks | 6:4e4df2f6157e | 155 | Burgerflip(); |
Frostworks | 6:4e4df2f6157e | 156 | } else if (turn == 0) { |
Frostworks | 6:4e4df2f6157e | 157 | M2_Speed = 0; |
Frostworks | 6:4e4df2f6157e | 158 | M3_Speed = 0; |
Frostworks | 6:4e4df2f6157e | 159 | } |
Frostworks | 6:4e4df2f6157e | 160 | if ((draailinks == false) && (draairechts == false)) { |
Frostworks | 6:4e4df2f6157e | 161 | M1_Speed = 0; |
Frostworks | 6:4e4df2f6157e | 162 | } |
Frostworks | 7:c7bdeee1bbac | 163 | pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); |
Frostworks | 6:4e4df2f6157e | 164 | /* pulses = 8400 */ |
Frostworks | 6:4e4df2f6157e | 165 | } |
Frostworks | 6:4e4df2f6157e | 166 | } |
Frostworks | 6:4e4df2f6157e | 167 | /* |
Frostworks | 3:dfb4b8d37bab | 168 | float GetReferenceVelocity() { |
Frostworks | 3:dfb4b8d37bab | 169 | // Returns reference velocity in rad/s. |
Frostworks | 3:dfb4b8d37bab | 170 | // Positive value means clockwise rotation. |
Frostworks | 3:dfb4b8d37bab | 171 | const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s |
Frostworks | 3:dfb4b8d37bab | 172 | if (a) { |
Frostworks | 3:dfb4b8d37bab | 173 | // Clockwise rotation |
Frostworks | 3:dfb4b8d37bab | 174 | referenceVelocity = potMeterIn * maxVelocity; |
Frostworks | 3:dfb4b8d37bab | 175 | |
Frostworks | 3:dfb4b8d37bab | 176 | } else { |
Frostworks | 3:dfb4b8d37bab | 177 | // Counterclockwise rotation |
Frostworks | 3:dfb4b8d37bab | 178 | referenceVelocity = -1*potMeterIn * maxVelocity; |
Frostworks | 3:dfb4b8d37bab | 179 | |
Frostworks | 3:dfb4b8d37bab | 180 | } |
Frostworks | 3:dfb4b8d37bab | 181 | return referenceVelocity; |
Frostworks | 2:4fb733fffd1f | 182 | } |
Frostworks | 2:4fb733fffd1f | 183 | |
Frostworks | 3:dfb4b8d37bab | 184 | void SetMotor1(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 185 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 186 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 187 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 188 | // within range |
Frostworks | 3:dfb4b8d37bab | 189 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 190 | motor1DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 191 | } else { |
Frostworks | 3:dfb4b8d37bab | 192 | motor1DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 193 | } |
Frostworks | 3:dfb4b8d37bab | 194 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 195 | motor1MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 196 | } else { |
Frostworks | 3:dfb4b8d37bab | 197 | motor1MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 198 | } |
Frostworks | 2:4fb733fffd1f | 199 | } |
Frostworks | 2:4fb733fffd1f | 200 | |
Frostworks | 3:dfb4b8d37bab | 201 | void SetMotor2(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 202 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 203 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 204 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 205 | // within range |
Frostworks | 3:dfb4b8d37bab | 206 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 207 | motor2DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 208 | } else { |
Frostworks | 3:dfb4b8d37bab | 209 | motor2DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 210 | } |
Frostworks | 3:dfb4b8d37bab | 211 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 212 | motor2MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 213 | } else { |
Frostworks | 3:dfb4b8d37bab | 214 | motor2MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 215 | } |
Frostworks | 2:4fb733fffd1f | 216 | } |
Frostworks | 3:dfb4b8d37bab | 217 | |
Frostworks | 3:dfb4b8d37bab | 218 | void SetMotor3(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 219 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 220 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 221 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 222 | // within range |
Frostworks | 3:dfb4b8d37bab | 223 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 224 | motor3DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 225 | } else { |
Frostworks | 3:dfb4b8d37bab | 226 | motor3DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 227 | } |
Frostworks | 3:dfb4b8d37bab | 228 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 229 | motor3MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 230 | } else { |
Frostworks | 3:dfb4b8d37bab | 231 | motor3MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 232 | } |
Frostworks | 3:dfb4b8d37bab | 233 | } |
Frostworks | 6:4e4df2f6157e | 234 | */ |