Alle functies van de motoren werken
Dependencies: MODSERIAL QEI mbed
Fork of worknotjet by
main.cpp@5:353ed56417a2, 2016-10-17 (annotated)
- Committer:
- Frostworks
- Date:
- Mon Oct 17 12:41:39 2016 +0000
- Revision:
- 5:353ed56417a2
- Parent:
- 4:7b8bf798bb72
- Child:
- 6:4e4df2f6157e
it works hello;
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 | 5:353ed56417a2 | 24 | bool turnthedamthing = 0; |
Frostworks | 5:353ed56417a2 | 25 | float waiter = 0.1; |
Frostworks | 3:dfb4b8d37bab | 26 | void GetDirections() |
Frostworks | 3:dfb4b8d37bab | 27 | { |
Frostworks | 5:353ed56417a2 | 28 | pc.baud(115200); |
Frostworks | 5:353ed56417a2 | 29 | if ((rechts == 0) && (links == 0) && (turnthedamthing == 0)) { |
Frostworks | 5:353ed56417a2 | 30 | draailinks = 0; |
Frostworks | 5:353ed56417a2 | 31 | draairechts = 0; |
Frostworks | 5:353ed56417a2 | 32 | turnthedamthing = 1; |
Frostworks | 5:353ed56417a2 | 33 | pc.printf("begin de actie \n \r "); |
Frostworks | 5:353ed56417a2 | 34 | wait(waiter); |
Frostworks | 5:353ed56417a2 | 35 | |
Frostworks | 5:353ed56417a2 | 36 | } else if ((rechts == 0) && (links == 0) && (turnthedamthing == 1)) { |
Frostworks | 5:353ed56417a2 | 37 | draailinks = 0; |
Frostworks | 5:353ed56417a2 | 38 | draairechts = 0; |
Frostworks | 5:353ed56417a2 | 39 | turnthedamthing = 0; |
Frostworks | 5:353ed56417a2 | 40 | pc.printf("breek de actie af \n \r "); |
Frostworks | 5:353ed56417a2 | 41 | wait(waiter); |
Frostworks | 5:353ed56417a2 | 42 | } else if ((rechts == 1) && (links == 1)) { |
Frostworks | 5:353ed56417a2 | 43 | |
Frostworks | 5:353ed56417a2 | 44 | } else if ((rechts == 1) && (draailinks == 0)) { |
Frostworks | 5:353ed56417a2 | 45 | /* if the right button is pressed and the motor isn't rotating to the left, |
Frostworks | 5:353ed56417a2 | 46 | then start rotating to the right etc*/ |
Frostworks | 3:dfb4b8d37bab | 47 | draairechts = !draairechts; |
Frostworks | 5:353ed56417a2 | 48 | pc.printf("draai naar rechts \n \r "); |
Frostworks | 5:353ed56417a2 | 49 | wait(waiter); |
Frostworks | 4:7b8bf798bb72 | 50 | } else if ((rechts == 1) && (draailinks == 1)) { |
Frostworks | 3:dfb4b8d37bab | 51 | draailinks = 0; |
Frostworks | 4:7b8bf798bb72 | 52 | draairechts = !draairechts; |
Frostworks | 5:353ed56417a2 | 53 | pc.printf("draai naar rechts na links \n \r "); |
Frostworks | 5:353ed56417a2 | 54 | wait(waiter); |
Frostworks | 5:353ed56417a2 | 55 | } else if ((links == 1) && (draairechts == 0)) { |
Frostworks | 4:7b8bf798bb72 | 56 | draailinks = !draailinks; |
Frostworks | 5:353ed56417a2 | 57 | pc.printf("draai naar links \n \r "); |
Frostworks | 5:353ed56417a2 | 58 | wait(waiter); |
Frostworks | 4:7b8bf798bb72 | 59 | } else if ((links == 1) && (draairechts == 1)) { |
Frostworks | 3:dfb4b8d37bab | 60 | draairechts = 0; |
Frostworks | 3:dfb4b8d37bab | 61 | draailinks = !draailinks; |
Frostworks | 5:353ed56417a2 | 62 | pc.printf("draai naar links na rechts \n \r "); |
Frostworks | 5:353ed56417a2 | 63 | wait(waiter); |
Frostworks | 3:dfb4b8d37bab | 64 | } |
Frostworks | 5:353ed56417a2 | 65 | wait(2*waiter); |
Frostworks | 3:dfb4b8d37bab | 66 | } |
Frostworks | 0:2b420376e01d | 67 | int main() |
Frostworks | 0:2b420376e01d | 68 | { |
Frostworks | 4:7b8bf798bb72 | 69 | |
Frostworks | 2:4fb733fffd1f | 70 | |
Frostworks | 3:dfb4b8d37bab | 71 | /* |
Frostworks | 3:dfb4b8d37bab | 72 | float GetReferenceVelocity() { |
Frostworks | 3:dfb4b8d37bab | 73 | // Returns reference velocity in rad/s. |
Frostworks | 3:dfb4b8d37bab | 74 | // Positive value means clockwise rotation. |
Frostworks | 3:dfb4b8d37bab | 75 | const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s |
Frostworks | 3:dfb4b8d37bab | 76 | if (a) { |
Frostworks | 3:dfb4b8d37bab | 77 | // Clockwise rotation |
Frostworks | 3:dfb4b8d37bab | 78 | referenceVelocity = potMeterIn * maxVelocity; |
Frostworks | 3:dfb4b8d37bab | 79 | |
Frostworks | 3:dfb4b8d37bab | 80 | } else { |
Frostworks | 3:dfb4b8d37bab | 81 | // Counterclockwise rotation |
Frostworks | 3:dfb4b8d37bab | 82 | referenceVelocity = -1*potMeterIn * maxVelocity; |
Frostworks | 3:dfb4b8d37bab | 83 | |
Frostworks | 3:dfb4b8d37bab | 84 | } |
Frostworks | 3:dfb4b8d37bab | 85 | return referenceVelocity; |
Frostworks | 2:4fb733fffd1f | 86 | } |
Frostworks | 2:4fb733fffd1f | 87 | |
Frostworks | 3:dfb4b8d37bab | 88 | void SetMotor1(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 89 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 90 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 91 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 92 | // within range |
Frostworks | 3:dfb4b8d37bab | 93 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 94 | motor1DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 95 | } else { |
Frostworks | 3:dfb4b8d37bab | 96 | motor1DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 97 | } |
Frostworks | 3:dfb4b8d37bab | 98 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 99 | motor1MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 100 | } else { |
Frostworks | 3:dfb4b8d37bab | 101 | motor1MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 102 | } |
Frostworks | 2:4fb733fffd1f | 103 | } |
Frostworks | 2:4fb733fffd1f | 104 | |
Frostworks | 3:dfb4b8d37bab | 105 | void SetMotor2(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 106 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 107 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 108 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 109 | // within range |
Frostworks | 3:dfb4b8d37bab | 110 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 111 | motor2DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 112 | } else { |
Frostworks | 3:dfb4b8d37bab | 113 | motor2DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 114 | } |
Frostworks | 3:dfb4b8d37bab | 115 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 116 | motor2MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 117 | } else { |
Frostworks | 3:dfb4b8d37bab | 118 | motor2MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 119 | } |
Frostworks | 2:4fb733fffd1f | 120 | } |
Frostworks | 3:dfb4b8d37bab | 121 | |
Frostworks | 3:dfb4b8d37bab | 122 | void SetMotor3(float motorValue) { |
Frostworks | 3:dfb4b8d37bab | 123 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frostworks | 3:dfb4b8d37bab | 124 | // bits for motor 1. Positive value makes motor rotating |
Frostworks | 3:dfb4b8d37bab | 125 | // clockwise. motorValues outside range are truncated to |
Frostworks | 3:dfb4b8d37bab | 126 | // within range |
Frostworks | 3:dfb4b8d37bab | 127 | if (motorValue >=0) { |
Frostworks | 3:dfb4b8d37bab | 128 | motor3DirectionPin=1; |
Frostworks | 3:dfb4b8d37bab | 129 | } else { |
Frostworks | 3:dfb4b8d37bab | 130 | motor3DirectionPin=0; |
Frostworks | 3:dfb4b8d37bab | 131 | } |
Frostworks | 3:dfb4b8d37bab | 132 | if (fabs(motorValue)>1) { |
Frostworks | 3:dfb4b8d37bab | 133 | motor3MagnitudePin = 1; |
Frostworks | 3:dfb4b8d37bab | 134 | } else { |
Frostworks | 3:dfb4b8d37bab | 135 | motor3MagnitudePin = fabs(motorValue); |
Frostworks | 3:dfb4b8d37bab | 136 | } |
Frostworks | 3:dfb4b8d37bab | 137 | } |
Frostworks | 3:dfb4b8d37bab | 138 | */ |
Frostworks | 1:833c73834749 | 139 | |
Frostworks | 0:2b420376e01d | 140 | while (true) { |
Frostworks | 2:4fb733fffd1f | 141 | GetDirections(); |
Frostworks | 4:7b8bf798bb72 | 142 | if (draairechts == true) { |
Frostworks | 4:7b8bf798bb72 | 143 | M1_Speed = 0.5; |
Frostworks | 4:7b8bf798bb72 | 144 | M1_rotate = 0; |
Frostworks | 4:7b8bf798bb72 | 145 | } else if (draailinks == true) { |
Frostworks | 4:7b8bf798bb72 | 146 | M1_Speed = 0.5; |
Frostworks | 4:7b8bf798bb72 | 147 | M1_rotate = 1; |
Frostworks | 4:7b8bf798bb72 | 148 | } else if ((draailinks == false) && (draairechts == false)) { |
Frostworks | 4:7b8bf798bb72 | 149 | M1_Speed = 0; |
Frostworks | 4:7b8bf798bb72 | 150 | } |
Frostworks | 5:353ed56417a2 | 151 | |
Frostworks | 4:7b8bf798bb72 | 152 | |
Frostworks | 4:7b8bf798bb72 | 153 | |
Frostworks | 4:7b8bf798bb72 | 154 | /* |
Frostworks | 3:dfb4b8d37bab | 155 | if ((draailinks == true) && (draairechts == false)) { |
Frostworks | 2:4fb733fffd1f | 156 | M1_Speed = 0.5; |
Frostworks | 3:dfb4b8d37bab | 157 | M1_rotate = 0; |
Frostworks | 3:dfb4b8d37bab | 158 | } else if ((draairechts == true) && (draailinks == false)) { |
Frostworks | 2:4fb733fffd1f | 159 | M1_Speed = 0.5; |
Frostworks | 3:dfb4b8d37bab | 160 | M1_rotate = 1; |
Frostworks | 2:4fb733fffd1f | 161 | } else { |
Frostworks | 2:4fb733fffd1f | 162 | M1_Speed = 0; |
Frostworks | 1:833c73834749 | 163 | } |
Frostworks | 4:7b8bf798bb72 | 164 | */ |
Frostworks | 3:dfb4b8d37bab | 165 | |
Frostworks | 2:4fb733fffd1f | 166 | /* |
Frostworks | 1:833c73834749 | 167 | float M2 = M2_rotate.read(); |
Frostworks | 1:833c73834749 | 168 | float potje1 = pot1.read(); |
Frostworks | 1:833c73834749 | 169 | float potje2 = pot2.read(); |
Frostworks | 1:833c73834749 | 170 | M1_Speed.write(potje1); |
Frostworks | 1:833c73834749 | 171 | M3_Speed.write(potje2); |
Frostworks | 0:2b420376e01d | 172 | wait(0.1); |
Frostworks | 1:833c73834749 | 173 | pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2); |
Frostworks | 2:4fb733fffd1f | 174 | */ |
Frostworks | 3:dfb4b8d37bab | 175 | |
Frostworks | 0:2b420376e01d | 176 | } |
Frostworks | 4:7b8bf798bb72 | 177 | |
Frostworks | 1:833c73834749 | 178 | } |