Met servo motor
Dependencies: MODSERIAL QEI mbed
Fork of Switch_motoren_motoren by
Diff: main.cpp
- Revision:
- 0:602d9d743817
- Child:
- 1:52a95e4b5662
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 21 13:41:09 2016 +0000 @@ -0,0 +1,164 @@ +#include "mbed.h" +#include "MODSERIAL.h" +#include "QEI.h" + +MODSERIAL pc(USBTX, USBRX); + +//Pinnen voor spieren +AnalogIn Spier1 (A0); +AnalogIn Spier2 (A1); +InterruptIn Spier3 (D2); + +//Pinnen voor motor +DigitalOut motor1direct (D4); +DigitalOut motor2direct (D7); +PwmOut motor1pwm (D5); +PwmOut motor2pwm (D6); +//QEI Encoder1(D10, D11, NC, 32); // met encoder onthouden waar je bent +//QEI Encoder2(D12, D13, NC, 32); // met encoder onthouden waar je bent + +//Define variables +volatile int indrukken = 0; +volatile int i = 0; +volatile float MV = 0; +//const float maxVelocity = 8.4; +//const float MotorGain=8.4; + +//Motorvalue is een waarde tussen -1 en 1 waarmee de motor een richting en een snelheid krijgt +float GetMotorValue() //We nemen aan dat je maar één spier tegelijkertijd kan aanspannen +{ + pc.printf("\n\n\n"); + if(Spier1 > 0.4f && Spier2 < 0.4f) //Spier komt boven de threshold uit en spier2 niet + { + MV = 0.5; + pc.printf("Spier 1 is aangespannen\r\n"); + } + else if(Spier1 < 0.4f && Spier2 > 0.4f) + { + MV = -0.5; + pc.printf("Spier 2 is aangespannen\r\n"); + } + else if(Spier1 <0.4f && Spier2 <0.4f) + { + MV = 0; + pc.printf("Geen spier is aangespannen\r\n"); + } + else + { + MV = 0; + pc.printf("Beide spieren zijn aangespannen\r\n"); + } + pc.printf("de motorvalue is %f\n\n\r",MV); + return MV; +} + +//Aan de hand van de motorvalue wordt de motor aangezet +void SetMotor1(float MV) +{ + //Given -1<=motorValue<=1, this sets the PWM and direction + // bits for motor 1. Positive value makes motor rotating + // clockwise. motorValues outside range are truncated to + // within range + if (MV >=0) + { + motor1direct = 1; + pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in + } + else + { + motor1direct = 0; + pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee + } + if (fabs(MV)>1) + { + motor1pwm = 1; + pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1 + } + else + { + motor1pwm = fabs(MV); + pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue + + } +} + +void SetMotor2(float MV) +{ + //Given -1<=motorValue<=1, this sets the PWM and direction + // bits for motor 1. Positive value makes motor rotating + // clockwise. motorValues outside range are truncated to + // within range + if (MV >=0) + { + motor2direct = 1; + pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in + } + else + { + motor2direct = 0; + pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee + } + if (fabs(MV)>1) + { + motor2pwm = 1; + pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1 + } + else + { + motor2pwm = fabs(MV); + pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue + + } +} + +void MeasureAndControl() +{ + float MV = GetMotorValue(); + switch(i) + { + case 0: + pc.printf("Motor 1\n\r"); + SetMotor1(MV); + SetMotor2(0); + //SetMotor3(0); + break; + case 1: + pc.printf("Motor 2\n\r"); + SetMotor1(0); + SetMotor2(MV); + //SetMotor3(0); + break; + case (2): + pc.printf("Motor 3\n\r"); + SetMotor1(0); + SetMotor2(0); + //SetMotor3(MV); + break; + } +} + +void count () +{ + indrukken ++; + pc.printf("Het knopje is %i x ingedrukt\n\r",indrukken); + i = indrukken%3; +} + + +int main() +{ + //motorpwm.period(1.0/1000.0); + pc.baud(115200); + pc.printf("START\n\r"); + Ticker motordraaien; + motordraaien.attach(MeasureAndControl,2); + Spier3.fall(count); + while(true){} +} + + + + + + +