Switch
Dependencies: MODSERIAL QEI mbed
main.cpp@2:d7e629ab497f, 2016-11-01 (annotated)
- Committer:
- LBeen
- Date:
- Tue Nov 01 14:55:42 2016 +0000
- Revision:
- 2:d7e629ab497f
- Parent:
- 1:52a95e4b5662
Ticker op 0.01 seconden, ticker werkt niet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LBeen | 0:602d9d743817 | 1 | #include "mbed.h" |
LBeen | 0:602d9d743817 | 2 | #include "MODSERIAL.h" |
LBeen | 0:602d9d743817 | 3 | #include "QEI.h" |
LBeen | 0:602d9d743817 | 4 | |
LBeen | 0:602d9d743817 | 5 | MODSERIAL pc(USBTX, USBRX); |
LBeen | 0:602d9d743817 | 6 | |
LBeen | 0:602d9d743817 | 7 | //Pinnen voor spieren |
LBeen | 0:602d9d743817 | 8 | AnalogIn Spier1 (A0); |
LBeen | 0:602d9d743817 | 9 | AnalogIn Spier2 (A1); |
LBeen | 0:602d9d743817 | 10 | InterruptIn Spier3 (D2); |
LBeen | 0:602d9d743817 | 11 | |
LBeen | 0:602d9d743817 | 12 | //Pinnen voor motor |
LBeen | 0:602d9d743817 | 13 | DigitalOut motor1direct (D4); |
LBeen | 0:602d9d743817 | 14 | DigitalOut motor2direct (D7); |
LBeen | 0:602d9d743817 | 15 | PwmOut motor1pwm (D5); |
LBeen | 0:602d9d743817 | 16 | PwmOut motor2pwm (D6); |
LBeen | 0:602d9d743817 | 17 | //QEI Encoder1(D10, D11, NC, 32); // met encoder onthouden waar je bent |
LBeen | 0:602d9d743817 | 18 | //QEI Encoder2(D12, D13, NC, 32); // met encoder onthouden waar je bent |
LBeen | 0:602d9d743817 | 19 | |
LBeen | 0:602d9d743817 | 20 | //Define variables |
LBeen | 0:602d9d743817 | 21 | volatile int indrukken = 0; |
LBeen | 0:602d9d743817 | 22 | volatile int i = 0; |
LBeen | 0:602d9d743817 | 23 | volatile float MV = 0; |
LBeen | 0:602d9d743817 | 24 | //const float maxVelocity = 8.4; |
LBeen | 0:602d9d743817 | 25 | //const float MotorGain=8.4; |
LBeen | 0:602d9d743817 | 26 | |
LBeen | 2:d7e629ab497f | 27 | //STOP KNOP |
LBeen | 2:d7e629ab497f | 28 | InterruptIn button(D3); |
LBeen | 2:d7e629ab497f | 29 | |
LBeen | 1:52a95e4b5662 | 30 | |
LBeen | 1:52a95e4b5662 | 31 | //Als straks de kalibratie af is, dan moet Spier1 > 0.4 en Spier2 >0.4 nog aangepast worden met boven threshold of niet |
LBeen | 1:52a95e4b5662 | 32 | |
LBeen | 2:d7e629ab497f | 33 | Ticker motordraaien; |
LBeen | 2:d7e629ab497f | 34 | |
LBeen | 0:602d9d743817 | 35 | //Motorvalue is een waarde tussen -1 en 1 waarmee de motor een richting en een snelheid krijgt |
LBeen | 0:602d9d743817 | 36 | float GetMotorValue() //We nemen aan dat je maar één spier tegelijkertijd kan aanspannen |
LBeen | 0:602d9d743817 | 37 | { |
LBeen | 0:602d9d743817 | 38 | pc.printf("\n\n\n"); |
LBeen | 0:602d9d743817 | 39 | if(Spier1 > 0.4f && Spier2 < 0.4f) //Spier komt boven de threshold uit en spier2 niet |
LBeen | 0:602d9d743817 | 40 | { |
LBeen | 0:602d9d743817 | 41 | MV = 0.5; |
LBeen | 0:602d9d743817 | 42 | pc.printf("Spier 1 is aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 43 | } |
LBeen | 0:602d9d743817 | 44 | else if(Spier1 < 0.4f && Spier2 > 0.4f) |
LBeen | 0:602d9d743817 | 45 | { |
LBeen | 0:602d9d743817 | 46 | MV = -0.5; |
LBeen | 0:602d9d743817 | 47 | pc.printf("Spier 2 is aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 48 | } |
LBeen | 0:602d9d743817 | 49 | else if(Spier1 <0.4f && Spier2 <0.4f) |
LBeen | 0:602d9d743817 | 50 | { |
LBeen | 0:602d9d743817 | 51 | MV = 0; |
LBeen | 0:602d9d743817 | 52 | pc.printf("Geen spier is aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 53 | } |
LBeen | 0:602d9d743817 | 54 | else |
LBeen | 0:602d9d743817 | 55 | { |
LBeen | 0:602d9d743817 | 56 | MV = 0; |
LBeen | 0:602d9d743817 | 57 | pc.printf("Beide spieren zijn aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 58 | } |
LBeen | 0:602d9d743817 | 59 | pc.printf("de motorvalue is %f\n\n\r",MV); |
LBeen | 0:602d9d743817 | 60 | return MV; |
LBeen | 0:602d9d743817 | 61 | } |
LBeen | 0:602d9d743817 | 62 | |
LBeen | 0:602d9d743817 | 63 | //Aan de hand van de motorvalue wordt de motor aangezet |
LBeen | 0:602d9d743817 | 64 | void SetMotor1(float MV) |
LBeen | 0:602d9d743817 | 65 | { |
LBeen | 0:602d9d743817 | 66 | //Given -1<=motorValue<=1, this sets the PWM and direction |
LBeen | 0:602d9d743817 | 67 | // bits for motor 1. Positive value makes motor rotating |
LBeen | 0:602d9d743817 | 68 | // clockwise. motorValues outside range are truncated to |
LBeen | 0:602d9d743817 | 69 | // within range |
LBeen | 0:602d9d743817 | 70 | if (MV >=0) |
LBeen | 0:602d9d743817 | 71 | { |
LBeen | 0:602d9d743817 | 72 | motor1direct = 1; |
LBeen | 0:602d9d743817 | 73 | pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in |
LBeen | 0:602d9d743817 | 74 | } |
LBeen | 0:602d9d743817 | 75 | else |
LBeen | 0:602d9d743817 | 76 | { |
LBeen | 0:602d9d743817 | 77 | motor1direct = 0; |
LBeen | 0:602d9d743817 | 78 | pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee |
LBeen | 0:602d9d743817 | 79 | } |
LBeen | 0:602d9d743817 | 80 | if (fabs(MV)>1) |
LBeen | 0:602d9d743817 | 81 | { |
LBeen | 0:602d9d743817 | 82 | motor1pwm = 1; |
LBeen | 0:602d9d743817 | 83 | pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1 |
LBeen | 0:602d9d743817 | 84 | } |
LBeen | 0:602d9d743817 | 85 | else |
LBeen | 0:602d9d743817 | 86 | { |
LBeen | 0:602d9d743817 | 87 | motor1pwm = fabs(MV); |
LBeen | 0:602d9d743817 | 88 | pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue |
LBeen | 0:602d9d743817 | 89 | } |
LBeen | 0:602d9d743817 | 90 | } |
LBeen | 0:602d9d743817 | 91 | |
LBeen | 0:602d9d743817 | 92 | void SetMotor2(float MV) |
LBeen | 0:602d9d743817 | 93 | { |
LBeen | 0:602d9d743817 | 94 | //Given -1<=motorValue<=1, this sets the PWM and direction |
LBeen | 0:602d9d743817 | 95 | // bits for motor 1. Positive value makes motor rotating |
LBeen | 0:602d9d743817 | 96 | // clockwise. motorValues outside range are truncated to |
LBeen | 0:602d9d743817 | 97 | // within range |
LBeen | 0:602d9d743817 | 98 | if (MV >=0) |
LBeen | 0:602d9d743817 | 99 | { |
LBeen | 0:602d9d743817 | 100 | motor2direct = 1; |
LBeen | 0:602d9d743817 | 101 | pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in |
LBeen | 0:602d9d743817 | 102 | } |
LBeen | 0:602d9d743817 | 103 | else |
LBeen | 0:602d9d743817 | 104 | { |
LBeen | 0:602d9d743817 | 105 | motor2direct = 0; |
LBeen | 0:602d9d743817 | 106 | pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee |
LBeen | 0:602d9d743817 | 107 | } |
LBeen | 0:602d9d743817 | 108 | if (fabs(MV)>1) |
LBeen | 0:602d9d743817 | 109 | { |
LBeen | 0:602d9d743817 | 110 | motor2pwm = 1; |
LBeen | 0:602d9d743817 | 111 | pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1 |
LBeen | 0:602d9d743817 | 112 | } |
LBeen | 0:602d9d743817 | 113 | else |
LBeen | 0:602d9d743817 | 114 | { |
LBeen | 0:602d9d743817 | 115 | motor2pwm = fabs(MV); |
LBeen | 0:602d9d743817 | 116 | pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue |
LBeen | 0:602d9d743817 | 117 | } |
LBeen | 0:602d9d743817 | 118 | } |
LBeen | 0:602d9d743817 | 119 | |
LBeen | 0:602d9d743817 | 120 | void MeasureAndControl() |
LBeen | 0:602d9d743817 | 121 | { |
LBeen | 0:602d9d743817 | 122 | float MV = GetMotorValue(); |
LBeen | 0:602d9d743817 | 123 | switch(i) |
LBeen | 0:602d9d743817 | 124 | { |
LBeen | 0:602d9d743817 | 125 | case 0: |
LBeen | 0:602d9d743817 | 126 | pc.printf("Motor 1\n\r"); |
LBeen | 0:602d9d743817 | 127 | SetMotor1(MV); |
LBeen | 0:602d9d743817 | 128 | SetMotor2(0); |
LBeen | 0:602d9d743817 | 129 | //SetMotor3(0); |
LBeen | 0:602d9d743817 | 130 | break; |
LBeen | 0:602d9d743817 | 131 | case 1: |
LBeen | 0:602d9d743817 | 132 | pc.printf("Motor 2\n\r"); |
LBeen | 0:602d9d743817 | 133 | SetMotor1(0); |
LBeen | 0:602d9d743817 | 134 | SetMotor2(MV); |
LBeen | 0:602d9d743817 | 135 | //SetMotor3(0); |
LBeen | 0:602d9d743817 | 136 | break; |
LBeen | 2:d7e629ab497f | 137 | case 2: |
LBeen | 0:602d9d743817 | 138 | pc.printf("Motor 3\n\r"); |
LBeen | 0:602d9d743817 | 139 | SetMotor1(0); |
LBeen | 0:602d9d743817 | 140 | SetMotor2(0); |
LBeen | 0:602d9d743817 | 141 | //SetMotor3(MV); |
LBeen | 0:602d9d743817 | 142 | break; |
LBeen | 0:602d9d743817 | 143 | } |
LBeen | 0:602d9d743817 | 144 | } |
LBeen | 0:602d9d743817 | 145 | |
LBeen | 0:602d9d743817 | 146 | void count () |
LBeen | 0:602d9d743817 | 147 | { |
LBeen | 0:602d9d743817 | 148 | indrukken ++; |
LBeen | 0:602d9d743817 | 149 | pc.printf("Het knopje is %i x ingedrukt\n\r",indrukken); |
LBeen | 0:602d9d743817 | 150 | i = indrukken%3; |
LBeen | 0:602d9d743817 | 151 | } |
LBeen | 0:602d9d743817 | 152 | |
LBeen | 2:d7e629ab497f | 153 | void reset() |
LBeen | 2:d7e629ab497f | 154 | { |
LBeen | 2:d7e629ab497f | 155 | motordraaien.detach(); |
LBeen | 2:d7e629ab497f | 156 | SetMotor1(0); |
LBeen | 2:d7e629ab497f | 157 | SetMotor2(0); |
LBeen | 2:d7e629ab497f | 158 | //SetMotor3(0); |
LBeen | 2:d7e629ab497f | 159 | } |
LBeen | 2:d7e629ab497f | 160 | |
LBeen | 0:602d9d743817 | 161 | int main() |
LBeen | 0:602d9d743817 | 162 | { |
LBeen | 0:602d9d743817 | 163 | //motorpwm.period(1.0/1000.0); |
LBeen | 0:602d9d743817 | 164 | pc.baud(115200); |
LBeen | 0:602d9d743817 | 165 | pc.printf("START\n\r"); |
LBeen | 2:d7e629ab497f | 166 | motordraaien.attach(MeasureAndControl,0.01); |
LBeen | 0:602d9d743817 | 167 | Spier3.fall(count); |
LBeen | 2:d7e629ab497f | 168 | button.fall(reset); |
LBeen | 0:602d9d743817 | 169 | while(true){} |
LBeen | 0:602d9d743817 | 170 | } |
LBeen | 0:602d9d743817 | 171 |