Switch
Dependencies: MODSERIAL QEI mbed
main.cpp
- Committer:
- LBeen
- Date:
- 2016-11-01
- Revision:
- 2:d7e629ab497f
- Parent:
- 1:52a95e4b5662
File content as of revision 2:d7e629ab497f:
#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; //STOP KNOP InterruptIn button(D3); //Als straks de kalibratie af is, dan moet Spier1 > 0.4 en Spier2 >0.4 nog aangepast worden met boven threshold of niet Ticker motordraaien; //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; } void reset() { motordraaien.detach(); SetMotor1(0); SetMotor2(0); //SetMotor3(0); } int main() { //motorpwm.period(1.0/1000.0); pc.baud(115200); pc.printf("START\n\r"); motordraaien.attach(MeasureAndControl,0.01); Spier3.fall(count); button.fall(reset); while(true){} }