Alle functies van de motoren werken
Dependencies: MODSERIAL QEI mbed
Fork of worknotjet by
main.cpp@4:7b8bf798bb72, 2016-10-14 (annotated)
- Committer:
- Frostworks
- Date:
- Fri Oct 14 10:34:17 2016 +0000
- Revision:
- 4:7b8bf798bb72
- Parent:
- 3:dfb4b8d37bab
- Child:
- 5:353ed56417a2
simpeler en werkender maar nog niet perfect;
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 | 0:2b420376e01d | 3 | |
Frostworks | 1:833c73834749 | 4 | DigitalOut M1_rotate(D2); // voltage only base rotation |
Frostworks | 1:833c73834749 | 5 | PwmOut M1_Speed(D3); // voltage only base rotation |
Frostworks | 1:833c73834749 | 6 | |
Frostworks | 1:833c73834749 | 7 | DigitalOut M2_rotate(D4); // encoder side pot 2 translation |
Frostworks | 1:833c73834749 | 8 | PwmOut M2_Speed(D5); // encoder side pot 2 translation |
Frostworks | 1:833c73834749 | 9 | |
Frostworks | 1:833c73834749 | 10 | DigitalOut M3_rotate(D7); // encoder side pot 1 spatel rotation |
Frostworks | 1:833c73834749 | 11 | PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation |
Frostworks | 0:2b420376e01d | 12 | |
Frostworks | 2:4fb733fffd1f | 13 | DigitalIn links(SW3); |
Frostworks | 2:4fb733fffd1f | 14 | DigitalIn rechts(SW2); |
Frostworks | 0:2b420376e01d | 15 | //DigitalOut M2_rotate(D6); |
Frostworks | 0:2b420376e01d | 16 | //PwmOut M2_Speed(D7); |
Frostworks | 0:2b420376e01d | 17 | |
Frostworks | 1:833c73834749 | 18 | AnalogIn pot1(A4); // pot 1 motor 1 |
Frostworks | 1:833c73834749 | 19 | AnalogIn pot2(A3); // pot 2 motor 3 |
Frostworks | 1:833c73834749 | 20 | |
Frostworks | 0:2b420376e01d | 21 | MODSERIAL pc(USBTX, USBRX); |
Frostworks | 3:dfb4b8d37bab | 22 | bool draairechts; |
Frostworks | 3:dfb4b8d37bab | 23 | bool draailinks; |
Frostworks | 0:2b420376e01d | 24 | |
Frostworks | 3:dfb4b8d37bab | 25 | void GetDirections() |
Frostworks | 3:dfb4b8d37bab | 26 | { |
Frostworks | 4:7b8bf798bb72 | 27 | if ((rechts == 1) && (links == 1)) |
Frostworks | 4:7b8bf798bb72 | 28 | {} |
Frostworks | 4:7b8bf798bb72 | 29 | else if ((rechts == 1) && (draailinks == 0)) { |
Frostworks | 3:dfb4b8d37bab | 30 | draairechts = !draairechts; |
Frostworks | 4:7b8bf798bb72 | 31 | } else if ((rechts == 1) && (draailinks == 1)) { |
Frostworks | 3:dfb4b8d37bab | 32 | draailinks = 0; |
Frostworks | 4:7b8bf798bb72 | 33 | draairechts = !draairechts; |
Frostworks | 4:7b8bf798bb72 | 34 | } else if (links == 1) { |
Frostworks | 4:7b8bf798bb72 | 35 | draailinks = !draailinks; |
Frostworks | 4:7b8bf798bb72 | 36 | } else if ((links == 1) && (draairechts == 1)) { |
Frostworks | 3:dfb4b8d37bab | 37 | draairechts = 0; |
Frostworks | 3:dfb4b8d37bab | 38 | draailinks = !draailinks; |
Frostworks | 3:dfb4b8d37bab | 39 | } |
Frostworks | 4:7b8bf798bb72 | 40 | wait(0.1); |
Frostworks | 3:dfb4b8d37bab | 41 | /*if ((rechts == 0) && (links == 0)) { |
Frostworks | 3:dfb4b8d37bab | 42 | draairechts = false; |
Frostworks | 3:dfb4b8d37bab | 43 | draailinks = false; |
Frostworks | 3:dfb4b8d37bab | 44 | } else if ((rechts == 1) && (draairechts ==false)) { //Als je rechts aanspant en hij draait nog niet naar rechts |
Frostworks | 3:dfb4b8d37bab | 45 | draairechts = true; |
Frostworks | 3:dfb4b8d37bab | 46 | draailinks = false; |
Frostworks | 3:dfb4b8d37bab | 47 | } else if ((rechts == 1) && (draairechts == true)) { //Als je rechts aanspant en hij draait al naar rechts |
Frostworks | 3:dfb4b8d37bab | 48 | draairechts = false; |
Frostworks | 3:dfb4b8d37bab | 49 | draailinks = false; |
Frostworks | 3:dfb4b8d37bab | 50 | } else if ((links == 1) && (draailinks == false)) { //Als je links aanspant en hij draait nog niet naar links |
Frostworks | 3:dfb4b8d37bab | 51 | draailinks = true; |
Frostworks | 3:dfb4b8d37bab | 52 | draairechts = false; |
Frostworks | 3:dfb4b8d37bab | 53 | } else if ((links == 1) && (draailinks == true)) { // Als je links aanspant en hij draait al naar links |
Frostworks | 3:dfb4b8d37bab | 54 | draailinks = false; |
Frostworks | 3:dfb4b8d37bab | 55 | draairechts = false; |
Frostworks | 3:dfb4b8d37bab | 56 | } |
Frostworks | 3:dfb4b8d37bab | 57 | |
Frostworks | 3:dfb4b8d37bab | 58 | else if ((links == 1 && draairechts == true)) { // Als je links aanspant en hij draait nog naar rechts |
Frostworks | 3:dfb4b8d37bab | 59 | draailinks = true; |
Frostworks | 3:dfb4b8d37bab | 60 | draairechts = false; |
Frostworks | 3:dfb4b8d37bab | 61 | } else if ((rechts == 1 && draailinks == true)) { // Als je rechts aanspant en hij draait nog naar links |
Frostworks | 3:dfb4b8d37bab | 62 | draairechts = true; |
Frostworks | 3:dfb4b8d37bab | 63 | draailinks = false; |
Frostworks | 3:dfb4b8d37bab | 64 | |
Frostworks | 3:dfb4b8d37bab | 65 | } else { |
Frostworks | 3:dfb4b8d37bab | 66 | draairechts = false; |
Frostworks | 3:dfb4b8d37bab | 67 | draailinks = false; |
Frostworks | 3:dfb4b8d37bab | 68 | } |
Frostworks | 3:dfb4b8d37bab | 69 | */ |
Frostworks | 3:dfb4b8d37bab | 70 | } |
Frostworks | 0:2b420376e01d | 71 | int main() |
Frostworks | 0:2b420376e01d | 72 | { |
Frostworks | 4:7b8bf798bb72 | 73 | |
Frostworks | 2:4fb733fffd1f | 74 | |
Frostworks | 3:dfb4b8d37bab | 75 | /* |
Frostworks | 3:dfb4b8d37bab | 76 | float GetReferenceVelocity() { |
Frostworks | 3:dfb4b8d37bab | 77 | // Returns reference velocity in rad/s. |
Frostworks | 3:dfb4b8d37bab | 78 | // Positive value means clockwise rotation. |
Frostworks | 3:dfb4b8d37bab | 79 | const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s |
Frostworks | 3:dfb4b8d37bab | 80 | if (a) { |
Frostworks | 3:dfb4b8d37bab | 81 | // Clockwise rotation |
Frostworks | 3:dfb4b8d37bab | 82 | referenceVelocity = potMeterIn * maxVelocity; |
Frostworks | 3:dfb4b8d37bab | 83 | |
Frostworks | 3:dfb4b8d37bab | 84 | } else { |
Frostworks | 3:dfb4b8d37bab | 85 | // Counterclockwise rotation |
Frostworks | 3:dfb4b8d37bab | 86 | referenceVelocity = -1*potMeterIn * maxVelocity; |
Frostworks | 3:dfb4b8d37bab | 87 | |
Frostworks | 3:dfb4b8d37bab | 88 | } |
Frostworks | 3:dfb4b8d37bab | 89 | return referenceVelocity; |
Frostworks | 2:4fb733fffd1f | 90 | } |
Frostworks | 2:4fb733fffd1f | 91 | |
Frostworks | 3:dfb4b8d37bab | 92 | void SetMotor1(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 93 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 94 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 95 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 96 | // within range |
Frostworks | 3:dfb4b8d37bab | 97 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 98 | motor1DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 99 | } else { |
Frostworks | 3:dfb4b8d37bab | 100 | motor1DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 101 | } |
Frostworks | 3:dfb4b8d37bab | 102 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 103 | motor1MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 104 | } else { |
Frostworks | 3:dfb4b8d37bab | 105 | motor1MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 106 | } |
Frostworks | 2:4fb733fffd1f | 107 | } |
Frostworks | 2:4fb733fffd1f | 108 | |
Frostworks | 3:dfb4b8d37bab | 109 | void SetMotor2(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 110 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 111 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 112 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 113 | // within range |
Frostworks | 3:dfb4b8d37bab | 114 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 115 | motor2DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 116 | } else { |
Frostworks | 3:dfb4b8d37bab | 117 | motor2DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 118 | } |
Frostworks | 3:dfb4b8d37bab | 119 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 120 | motor2MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 121 | } else { |
Frostworks | 3:dfb4b8d37bab | 122 | motor2MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 123 | } |
Frostworks | 2:4fb733fffd1f | 124 | } |
Frostworks | 3:dfb4b8d37bab | 125 | |
Frostworks | 3:dfb4b8d37bab | 126 | void SetMotor3(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 127 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 128 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 129 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 130 | // within range |
Frostworks | 3:dfb4b8d37bab | 131 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 132 | motor3DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 133 | } else { |
Frostworks | 3:dfb4b8d37bab | 134 | motor3DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 135 | } |
Frostworks | 3:dfb4b8d37bab | 136 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 137 | motor3MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 138 | } else { |
Frostworks | 3:dfb4b8d37bab | 139 | motor3MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 140 | } |
Frostworks | 3:dfb4b8d37bab | 141 | } |
Frostworks | 3:dfb4b8d37bab | 142 | */ |
Frostworks | 1:833c73834749 | 143 | |
Frostworks | 0:2b420376e01d | 144 | while (true) { |
Frostworks | 2:4fb733fffd1f | 145 | GetDirections(); |
Frostworks | 4:7b8bf798bb72 | 146 | if (draairechts == true) { |
Frostworks | 4:7b8bf798bb72 | 147 | M1_Speed = 0.5; |
Frostworks | 4:7b8bf798bb72 | 148 | M1_rotate = 0; |
Frostworks | 4:7b8bf798bb72 | 149 | } else if (draailinks == true) { |
Frostworks | 4:7b8bf798bb72 | 150 | M1_Speed = 0.5; |
Frostworks | 4:7b8bf798bb72 | 151 | M1_rotate = 1; |
Frostworks | 4:7b8bf798bb72 | 152 | } else if ((draailinks == false) && (draairechts == false)) { |
Frostworks | 4:7b8bf798bb72 | 153 | M1_Speed = 0; |
Frostworks | 4:7b8bf798bb72 | 154 | } |
Frostworks | 4:7b8bf798bb72 | 155 | |
Frostworks | 4:7b8bf798bb72 | 156 | |
Frostworks | 4:7b8bf798bb72 | 157 | |
Frostworks | 4:7b8bf798bb72 | 158 | /* |
Frostworks | 3:dfb4b8d37bab | 159 | if ((draailinks == true) && (draairechts == false)) { |
Frostworks | 2:4fb733fffd1f | 160 | M1_Speed = 0.5; |
Frostworks | 3:dfb4b8d37bab | 161 | M1_rotate = 0; |
Frostworks | 3:dfb4b8d37bab | 162 | } else if ((draairechts == true) && (draailinks == false)) { |
Frostworks | 2:4fb733fffd1f | 163 | M1_Speed = 0.5; |
Frostworks | 3:dfb4b8d37bab | 164 | M1_rotate = 1; |
Frostworks | 2:4fb733fffd1f | 165 | } else { |
Frostworks | 2:4fb733fffd1f | 166 | M1_Speed = 0; |
Frostworks | 1:833c73834749 | 167 | } |
Frostworks | 4:7b8bf798bb72 | 168 | */ |
Frostworks | 3:dfb4b8d37bab | 169 | |
Frostworks | 2:4fb733fffd1f | 170 | /* |
Frostworks | 1:833c73834749 | 171 | float M2 = M2_rotate.read(); |
Frostworks | 1:833c73834749 | 172 | float potje1 = pot1.read(); |
Frostworks | 1:833c73834749 | 173 | float potje2 = pot2.read(); |
Frostworks | 1:833c73834749 | 174 | M1_Speed.write(potje1); |
Frostworks | 1:833c73834749 | 175 | M3_Speed.write(potje2); |
Frostworks | 0:2b420376e01d | 176 | wait(0.1); |
Frostworks | 1:833c73834749 | 177 | pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2); |
Frostworks | 2:4fb733fffd1f | 178 | */ |
Frostworks | 3:dfb4b8d37bab | 179 | |
Frostworks | 0:2b420376e01d | 180 | } |
Frostworks | 4:7b8bf798bb72 | 181 | |
Frostworks | 1:833c73834749 | 182 | } |