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