Switch
Dependencies: MODSERIAL QEI mbed
main.cpp@0:602d9d743817, 2016-10-21 (annotated)
- Committer:
- LBeen
- Date:
- Fri Oct 21 13:41:09 2016 +0000
- Revision:
- 0:602d9d743817
- Child:
- 1:52a95e4b5662
HET WERKT :)
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 | 0:602d9d743817 | 27 | //Motorvalue is een waarde tussen -1 en 1 waarmee de motor een richting en een snelheid krijgt |
LBeen | 0:602d9d743817 | 28 | float GetMotorValue() //We nemen aan dat je maar één spier tegelijkertijd kan aanspannen |
LBeen | 0:602d9d743817 | 29 | { |
LBeen | 0:602d9d743817 | 30 | pc.printf("\n\n\n"); |
LBeen | 0:602d9d743817 | 31 | if(Spier1 > 0.4f && Spier2 < 0.4f) //Spier komt boven de threshold uit en spier2 niet |
LBeen | 0:602d9d743817 | 32 | { |
LBeen | 0:602d9d743817 | 33 | MV = 0.5; |
LBeen | 0:602d9d743817 | 34 | pc.printf("Spier 1 is aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 35 | } |
LBeen | 0:602d9d743817 | 36 | else if(Spier1 < 0.4f && Spier2 > 0.4f) |
LBeen | 0:602d9d743817 | 37 | { |
LBeen | 0:602d9d743817 | 38 | MV = -0.5; |
LBeen | 0:602d9d743817 | 39 | pc.printf("Spier 2 is aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 40 | } |
LBeen | 0:602d9d743817 | 41 | else if(Spier1 <0.4f && Spier2 <0.4f) |
LBeen | 0:602d9d743817 | 42 | { |
LBeen | 0:602d9d743817 | 43 | MV = 0; |
LBeen | 0:602d9d743817 | 44 | pc.printf("Geen spier is aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 45 | } |
LBeen | 0:602d9d743817 | 46 | else |
LBeen | 0:602d9d743817 | 47 | { |
LBeen | 0:602d9d743817 | 48 | MV = 0; |
LBeen | 0:602d9d743817 | 49 | pc.printf("Beide spieren zijn aangespannen\r\n"); |
LBeen | 0:602d9d743817 | 50 | } |
LBeen | 0:602d9d743817 | 51 | pc.printf("de motorvalue is %f\n\n\r",MV); |
LBeen | 0:602d9d743817 | 52 | return MV; |
LBeen | 0:602d9d743817 | 53 | } |
LBeen | 0:602d9d743817 | 54 | |
LBeen | 0:602d9d743817 | 55 | //Aan de hand van de motorvalue wordt de motor aangezet |
LBeen | 0:602d9d743817 | 56 | void SetMotor1(float MV) |
LBeen | 0:602d9d743817 | 57 | { |
LBeen | 0:602d9d743817 | 58 | //Given -1<=motorValue<=1, this sets the PWM and direction |
LBeen | 0:602d9d743817 | 59 | // bits for motor 1. Positive value makes motor rotating |
LBeen | 0:602d9d743817 | 60 | // clockwise. motorValues outside range are truncated to |
LBeen | 0:602d9d743817 | 61 | // within range |
LBeen | 0:602d9d743817 | 62 | if (MV >=0) |
LBeen | 0:602d9d743817 | 63 | { |
LBeen | 0:602d9d743817 | 64 | motor1direct = 1; |
LBeen | 0:602d9d743817 | 65 | pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in |
LBeen | 0:602d9d743817 | 66 | } |
LBeen | 0:602d9d743817 | 67 | else |
LBeen | 0:602d9d743817 | 68 | { |
LBeen | 0:602d9d743817 | 69 | motor1direct = 0; |
LBeen | 0:602d9d743817 | 70 | pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee |
LBeen | 0:602d9d743817 | 71 | } |
LBeen | 0:602d9d743817 | 72 | if (fabs(MV)>1) |
LBeen | 0:602d9d743817 | 73 | { |
LBeen | 0:602d9d743817 | 74 | motor1pwm = 1; |
LBeen | 0:602d9d743817 | 75 | pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1 |
LBeen | 0:602d9d743817 | 76 | } |
LBeen | 0:602d9d743817 | 77 | else |
LBeen | 0:602d9d743817 | 78 | { |
LBeen | 0:602d9d743817 | 79 | motor1pwm = fabs(MV); |
LBeen | 0:602d9d743817 | 80 | 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 | 81 | |
LBeen | 0:602d9d743817 | 82 | } |
LBeen | 0:602d9d743817 | 83 | } |
LBeen | 0:602d9d743817 | 84 | |
LBeen | 0:602d9d743817 | 85 | void SetMotor2(float MV) |
LBeen | 0:602d9d743817 | 86 | { |
LBeen | 0:602d9d743817 | 87 | //Given -1<=motorValue<=1, this sets the PWM and direction |
LBeen | 0:602d9d743817 | 88 | // bits for motor 1. Positive value makes motor rotating |
LBeen | 0:602d9d743817 | 89 | // clockwise. motorValues outside range are truncated to |
LBeen | 0:602d9d743817 | 90 | // within range |
LBeen | 0:602d9d743817 | 91 | if (MV >=0) |
LBeen | 0:602d9d743817 | 92 | { |
LBeen | 0:602d9d743817 | 93 | motor2direct = 1; |
LBeen | 0:602d9d743817 | 94 | pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in |
LBeen | 0:602d9d743817 | 95 | } |
LBeen | 0:602d9d743817 | 96 | else |
LBeen | 0:602d9d743817 | 97 | { |
LBeen | 0:602d9d743817 | 98 | motor2direct = 0; |
LBeen | 0:602d9d743817 | 99 | pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee |
LBeen | 0:602d9d743817 | 100 | } |
LBeen | 0:602d9d743817 | 101 | if (fabs(MV)>1) |
LBeen | 0:602d9d743817 | 102 | { |
LBeen | 0:602d9d743817 | 103 | motor2pwm = 1; |
LBeen | 0:602d9d743817 | 104 | pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1 |
LBeen | 0:602d9d743817 | 105 | } |
LBeen | 0:602d9d743817 | 106 | else |
LBeen | 0:602d9d743817 | 107 | { |
LBeen | 0:602d9d743817 | 108 | motor2pwm = fabs(MV); |
LBeen | 0:602d9d743817 | 109 | 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 | 110 | |
LBeen | 0:602d9d743817 | 111 | } |
LBeen | 0:602d9d743817 | 112 | } |
LBeen | 0:602d9d743817 | 113 | |
LBeen | 0:602d9d743817 | 114 | void MeasureAndControl() |
LBeen | 0:602d9d743817 | 115 | { |
LBeen | 0:602d9d743817 | 116 | float MV = GetMotorValue(); |
LBeen | 0:602d9d743817 | 117 | switch(i) |
LBeen | 0:602d9d743817 | 118 | { |
LBeen | 0:602d9d743817 | 119 | case 0: |
LBeen | 0:602d9d743817 | 120 | pc.printf("Motor 1\n\r"); |
LBeen | 0:602d9d743817 | 121 | SetMotor1(MV); |
LBeen | 0:602d9d743817 | 122 | SetMotor2(0); |
LBeen | 0:602d9d743817 | 123 | //SetMotor3(0); |
LBeen | 0:602d9d743817 | 124 | break; |
LBeen | 0:602d9d743817 | 125 | case 1: |
LBeen | 0:602d9d743817 | 126 | pc.printf("Motor 2\n\r"); |
LBeen | 0:602d9d743817 | 127 | SetMotor1(0); |
LBeen | 0:602d9d743817 | 128 | SetMotor2(MV); |
LBeen | 0:602d9d743817 | 129 | //SetMotor3(0); |
LBeen | 0:602d9d743817 | 130 | break; |
LBeen | 0:602d9d743817 | 131 | case (2): |
LBeen | 0:602d9d743817 | 132 | pc.printf("Motor 3\n\r"); |
LBeen | 0:602d9d743817 | 133 | SetMotor1(0); |
LBeen | 0:602d9d743817 | 134 | SetMotor2(0); |
LBeen | 0:602d9d743817 | 135 | //SetMotor3(MV); |
LBeen | 0:602d9d743817 | 136 | break; |
LBeen | 0:602d9d743817 | 137 | } |
LBeen | 0:602d9d743817 | 138 | } |
LBeen | 0:602d9d743817 | 139 | |
LBeen | 0:602d9d743817 | 140 | void count () |
LBeen | 0:602d9d743817 | 141 | { |
LBeen | 0:602d9d743817 | 142 | indrukken ++; |
LBeen | 0:602d9d743817 | 143 | pc.printf("Het knopje is %i x ingedrukt\n\r",indrukken); |
LBeen | 0:602d9d743817 | 144 | i = indrukken%3; |
LBeen | 0:602d9d743817 | 145 | } |
LBeen | 0:602d9d743817 | 146 | |
LBeen | 0:602d9d743817 | 147 | |
LBeen | 0:602d9d743817 | 148 | int main() |
LBeen | 0:602d9d743817 | 149 | { |
LBeen | 0:602d9d743817 | 150 | //motorpwm.period(1.0/1000.0); |
LBeen | 0:602d9d743817 | 151 | pc.baud(115200); |
LBeen | 0:602d9d743817 | 152 | pc.printf("START\n\r"); |
LBeen | 0:602d9d743817 | 153 | Ticker motordraaien; |
LBeen | 0:602d9d743817 | 154 | motordraaien.attach(MeasureAndControl,2); |
LBeen | 0:602d9d743817 | 155 | Spier3.fall(count); |
LBeen | 0:602d9d743817 | 156 | while(true){} |
LBeen | 0:602d9d743817 | 157 | } |
LBeen | 0:602d9d743817 | 158 | |
LBeen | 0:602d9d743817 | 159 | |
LBeen | 0:602d9d743817 | 160 | |
LBeen | 0:602d9d743817 | 161 | |
LBeen | 0:602d9d743817 | 162 | |
LBeen | 0:602d9d743817 | 163 | |
LBeen | 0:602d9d743817 | 164 |