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){}  
+}
+
+
+
+
+
+
+