Met servo motor
Dependencies: MODSERIAL QEI mbed
Fork of Switch_motoren_motoren by
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
Generated on Wed Aug 24 2022 10:30:57 by 1.7.2