main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "QEI.h"
00004 
00005 MODSERIAL pc(USBTX, USBRX);
00006 
00007 //Pinnen voor spieren
00008 AnalogIn Spier1 (A0);
00009 AnalogIn Spier2 (A1);
00010 InterruptIn Spier3 (D2);
00011 
00012 //Pinnen voor motor
00013 DigitalOut motor1direct (D4);
00014 DigitalOut motor2direct (D7);
00015 PwmOut motor1pwm (D5);
00016 PwmOut motor2pwm (D6);
00017 //QEI Encoder1(D10, D11, NC, 32); // met encoder onthouden waar je bent
00018 //QEI Encoder2(D12, D13, NC, 32); // met encoder onthouden waar je bent
00019 
00020 //Pin voor de servo
00021 PwmOut servo(D9);
00022 
00023 //Define variables
00024 volatile int indrukken = 0;
00025 volatile int i = 0;
00026 volatile float MV = 0;
00027 //const float maxVelocity = 8.4;
00028 //const float MotorGain=8.4;
00029 
00030 
00031 //Als straks de kalibratie af is, dan moet Spier1 > 0.4 en Spier2 >0.4 nog aangepast worden met boven threshold of niet
00032 
00033 //Motorvalue is een waarde tussen -1 en 1 waarmee de motor een richting en een snelheid krijgt
00034 float GetMotorValue() //We nemen aan dat je maar één spier tegelijkertijd kan aanspannen
00035 {
00036     pc.printf("\n\n\n");
00037     if(Spier1 > 0.4f && Spier2 < 0.4f) //Spier komt boven de threshold uit en spier2 niet
00038     {
00039         MV = 0.5;
00040         pc.printf("Spier 1 is aangespannen\r\n");
00041     }
00042     else if(Spier1 < 0.4f && Spier2 > 0.4f)
00043     {
00044         MV = -0.5;
00045         pc.printf("Spier 2 is aangespannen\r\n");
00046     }
00047     else if(Spier1 <0.4f && Spier2 <0.4f)
00048     {
00049         MV = 0;
00050         pc.printf("Geen spier is aangespannen\r\n");
00051     }
00052     else
00053     {
00054         MV = 0;
00055         pc.printf("Beide spieren zijn aangespannen\r\n");
00056     }
00057     pc.printf("de motorvalue is %f\n\n\r",MV);
00058     return MV;
00059 }
00060 
00061 //Aan de hand van de motorvalue wordt de motor aangezet
00062 void SetMotor1(float MV)
00063 {
00064     //Given -1<=motorValue<=1, this sets the PWM and direction
00065     // bits for motor 1. Positive value makes motor rotating
00066     // clockwise. motorValues outside range are truncated to
00067     // within range
00068     if (MV >=0)
00069         {
00070             motor1direct = 1;
00071             pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in
00072         }
00073     else
00074         {
00075             motor1direct = 0;
00076             pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee
00077         }
00078     if (fabs(MV)>1) 
00079     {
00080         motor1pwm = 1;
00081         pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1
00082     }
00083     else
00084     {
00085         motor1pwm = fabs(MV);
00086         pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue
00087     }
00088 }
00089 
00090 void SetMotor2(float MV)
00091 {
00092     //Given -1<=motorValue<=1, this sets the PWM and direction
00093     // bits for motor 1. Positive value makes motor rotating
00094     // clockwise. motorValues outside range are truncated to
00095     // within range
00096     if (MV >=0)
00097         {
00098             motor2direct = 1;
00099             pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in
00100         }
00101     else
00102         {
00103             motor2direct = 0;
00104             pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee
00105         }
00106     if (fabs(MV)>1) 
00107     {
00108         motor2pwm = 1;
00109         pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1
00110     }
00111     else
00112     {
00113         motor2pwm = fabs(MV);
00114         pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue
00115     }
00116 }
00117 
00118 void SetMotor3(float MV)
00119 {
00120     if (MV > 0)
00121         {
00122             servo = 0.04;
00123             pc.printf("servo = 0.01\n\r"); //De servo draait maximaal rechtsom de klep dus open
00124         }
00125     else if (MV = 0)
00126         {
00127             servo = servo;
00128             pc.printf("servo = servo\n\r");
00129         }
00130     else (MV < 0)
00131         {
00132             servo = 0.08;
00133             pc.printf("servo = 0.15\n\r");
00134         }
00135 }
00136 
00137 void MeasureAndControl()
00138 {
00139     float MV = GetMotorValue();
00140     switch(i)
00141     {
00142         case 0:
00143             pc.printf("Motor 1\n\r");
00144             SetMotor1(MV);
00145             SetMotor2(0);
00146             //SetMotor3(0);
00147             break;
00148         case 1:
00149             pc.printf("Motor 2\n\r");
00150             SetMotor1(0);
00151             SetMotor2(MV);
00152             //SetMotor3(0);
00153             break;
00154         case (2):
00155             pc.printf("Motor 3\n\r");
00156             SetMotor1(0);
00157             SetMotor2(0);
00158             //SetMotor3(MV);
00159             break;
00160     }
00161 }
00162 
00163 void count ()
00164 {
00165     indrukken ++;
00166     pc.printf("Het knopje is %i x ingedrukt\n\r",indrukken);
00167     i = indrukken%3;
00168 }
00169 
00170 int main()
00171 {
00172     //motorpwm.period(1.0/1000.0);
00173     pc.baud(115200);
00174     pc.printf("START\n\r");
00175     Ticker motordraaien;
00176     motordraaien.attach(MeasureAndControl,2);
00177     Spier3.fall(count);
00178     servo.period(0.020) //servo periode 20ms
00179     while(true){}  
00180 }
00181