Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 42:ca4bbc3e0239
- Parent:
- 41:dcfe6c86e3f5
--- a/main.cpp Tue Nov 05 16:09:52 2019 +0000
+++ b/main.cpp Tue Nov 05 20:18:55 2019 +0000
@@ -172,13 +172,13 @@
// VOIDS
void emergency() // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld
-{ // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna
- loop_ticker.detach(); // het script opnieuw moet worden opgestart.
+{ // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna
+ loop_ticker.detach(); // het script opnieuw moet worden opgestart.
motor1.write(0);
motor2.write(0);
pc.printf("Ik ga exploderen!!!\r\n");
}
-
+
void motors_off() // Functie waarbij beide motoren worden uitgezet.
{
motor1.write(0);
@@ -262,9 +262,9 @@
Motor_2_position = Joint_1_position + Joint_2_position;
}
-void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald.
-{ // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input
-// Proportional part: // voor de motorsnelheden gebruikt.
+void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald.
+{ // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input
+// Proportional part: // voor de motorsnelheden gebruikt.
theta_k_1= Kp * error_M1;
theta_k_2= Kp * error_M2;
@@ -300,8 +300,8 @@
void Controlling_system() // Hoofdfunctie die wordt toegepast tijdens de operation mode. Een gewenste snelheid in x- en y-richting wordt
{ // hiervoor opgesteld en is vervolgens de input voor de inverse kinematica functie. Door middel van een gewenste
- Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de
- // PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie
+ Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de
+ // PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie
error_M1 = Motor_1_position + theta_h_1_rad; // aangeroepen die ervoor zorgt dat de motoren in de juiste richting gaan draaien.
error_M2 = Motor_2_position + theta_h_2_rad;
@@ -315,38 +315,38 @@
Define_motor_dir();
}
-void servo()
+void servo_horizontal() // Functie die ervoor zorgt dat de spatel horizontaal wordt gehouden door de servo motor in de operation mode.
{
q1_ser= q1*604.7887837;
q2_ser= q2*604.7887837;
+ theta_sout=theta_f+q1_ser+q2_ser;
- if (servo_button_pressed.read()== false) {
- theta_sout=theta_f+q1_ser+q2_ser+400;
- if (theta_sout>=2400) {
- theta_sout=2400;
- }
- if (theta_sout<=500) {
- theta_sout=500;
- } else {
- theta_sout;
- }
+ if (theta_sout>=2400) {
+ theta_sout=2400;
+ }
+ if (theta_sout<=500) {
+ theta_sout=500;
+
mijnServo.SetPosition(theta_sout);
- pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout);
+ pc.printf("De servo staat op %f graden\n\r", theta_sout);
+ }
+}
+
+void servo_flip() // Functie die ervoor zorgt dat de spatel flipt, wanneer er een button wordt ingedrukt. Wordt eveneens gerealiseerd
+{ // door de servo motor.
+ theta_sout=theta_f+q1_ser+q2_ser+400;
+
+ if (theta_sout>=2400) {
+ theta_sout=2400;
+ }
+ if (theta_sout<=500) {
+ theta_sout=500;
+ } else {
+ theta_sout;
}
- else {
- theta_sout=theta_f+q1_ser+q2_ser;
- if (theta_sout>=2400) {
- theta_sout=2400;
- }
- if (theta_sout<=500) {
- theta_sout=500;
- } else {
- theta_sout;
- }
- mijnServo.SetPosition(theta_sout);
- pc.printf("De servo staat op %f graden.\n\r", theta_sout);
- }
+ mijnServo.SetPosition(theta_sout);
+ pc.printf("De servo staat op %f graden, in de klapstand\n\r", theta_sout);
}
// Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine
@@ -360,8 +360,7 @@
volatile bool loop_done = true;
-// Finite state machine programming (calibration servo motor?)
-void ProcessStateMachine(void)
+void ProcessStateMachine(void) // Programmeren van de finite state machine.
{
if (!loop_done) {
pc.printf("There is a timing problem\r\n");
@@ -436,9 +435,9 @@
}
break;
- case Calib_motor: // In deze state wordt de robot arm met de hand in de juiste referentie (home) positie geplaatst.
- // Wanneer dit is gebeurd, wordt de motor kalibratie knop ingedrukt. De offset wordt dan ingesteld als
- if (stateChanged) { // het huidige aantal counts. Door deze offset in de komende metingen af te trekken van het aantal counts
+ case Calib_motor: // In deze state wordt de robot arm met de hand in de juiste referentie (home) positie geplaatst.
+ // Wanneer dit is gebeurd, wordt de motor kalibratie knop ingedrukt. De offset wordt dan ingesteld als
+ if (stateChanged) { // het huidige aantal counts. Door deze offset in de komende metingen af te trekken van het aantal counts
pc.printf("Zet motoren handmatig in home positie\r\n"); // op dat moment, wordt ervoor gezorgd dat de motorhoeken in de referentiepositie gelijk zijn aan nul.
stateChanged = false;
}
@@ -455,7 +454,7 @@
}
break;
- case Calib_EMG: // In deze state wordt een kalibratie uitgevoerd van de EMG-signalen. Hiervoor wordt een bool (calib)
+ case Calib_EMG: // In deze state wordt een kalibratie uitgevoerd van de EMG-signalen. Hiervoor wordt een bool (calib)
// samen met een teller (i_calib) in werking gezet, die ervoor zorgt dat de eerder beschreven EMG-kalibratie
if (stateChanged) { // wordt doorlopen. De spieren worden tijdens de kalibratie gedurende 5 seconden maximaal aangespannen.
i_calib = 0; // Na deze 5 seconden wordt doorgegaan naar de volgende state. De output van deze kalibratie is een gemiddelde
@@ -477,7 +476,7 @@
break;
case Homing: // In deze state wordt de homing uitgevoerd. De motoren draaien hier op een langzame snelheid, todat de gewenste referentiepositie
- // wordt bereikt. In dit geval worden de motorsnelheden op 0 gezet. Indien beide motoren de juiste posities hebben bereikt, wordt
+ // wordt bereikt. In dit geval worden de motorsnelheden op 0 gezet. Indien beide motoren de juiste posities hebben bereikt, wordt
if (stateChanged) { // overgegaan naar de volgende state. De eerste keer dat de ProcessStateMachine in de homing state belandt, is dit al het geval,
stateChanged = false; // waardoor gelijk doorgegaan kan worden naar de operation mode.
}
@@ -497,7 +496,7 @@
case Operation_mode:
if (stateChanged) { // Aan het begin van de operation mode wordt aan allerlei variabelen een beginwaarde van 0 toegekend. Dit wordt één keer gedaan
- motors_off(); // (stateChanged), waarna deze waarden verderop in deze state worden overschreven. In het geval dat er tussendoor teruggegaan is
+ motors_off(); // (stateChanged), waarna deze waarden verderop in deze state worden overschreven. In het geval dat er tussendoor teruggegaan is
Joint_1_position = 0; // naar een andere state, wordt op deze manier elke keer weer begonnen met de juiste beginwaarden.
Joint_2_position = 0;
Joint_1_position_prev = Joint_1_position;
@@ -513,6 +512,12 @@
stateChanged = false;
}
+ servo_horizontal(); // Zorgt ervoor dat de spatel horizontaal wordt gehouden.
+
+ if (servo_button_pressed.read()== false) {
+ servo_flip(); // Zorgt ervoor dat de spatel flipt, wanneer de servo knop wordt ingedrukt.
+ }
+
if (Power_button_pressed.read() == false) { // Wanneer de power button wordt ingedrukt, worden de motoren uitgezet.
motors_off();
currentState = Motors_off;
@@ -522,18 +527,18 @@
if (Emergency_button_pressed.read() == false) { // Wanneer de emergency button wordt ingedrukt, wordt de emergency functie aangeroepen.
emergency();
}
- if (Motor_calib_button_pressed.read() == false) { // Wanneer tijdens de operation mode teruggegaan moet worden naar de referentie positie, kan de button voor de motor kalibratie
- motors_off(); // worden ingedrukt. In dit geval is het niet nodig om de motoren opnieuw te kalibreren, dus kan direct teruggegaan worden naar
+ if (Motor_calib_button_pressed.read() == false) { // Wanneer tijdens de operation mode teruggegaan moet worden naar de referentie positie, kan de button voor de motor kalibratie
+ motors_off(); // worden ingedrukt. In dit geval is het niet nodig om de motoren opnieuw te kalibreren, dus kan direct teruggegaan worden naar
currentState = Homing; // de state Homing. De motoren bewegen dan totdat de referentie positie weer wordt bereikt.
stateChanged = true;
pc.printf("Terug naar de state Homing\r\n");
}
- if (normalized_EMG_biceps_right >= 0.3) { // Vanaf hier begint de daadwerkelijke besturing van de robot arm door middel van EMG signalen. Indien de rechter biceps wordt
- // aangespannen, zal het genormaliseerde EMG signaal boven 0.3 uitkomen. Wanneer tegelijkertijd de kuit wordt aangespannen is er
+ if (normalized_EMG_biceps_right >= 0.3) { // Vanaf hier begint de daadwerkelijke besturing van de robot arm door middel van EMG signalen. Indien de rechter biceps wordt
+ // aangespannen, zal het genormaliseerde EMG signaal boven 0.3 uitkomen. Wanneer tegelijkertijd de kuit wordt aangespannen is er
if (normalized_EMG_calf < 0.3) { // een gewenste negatieve snelheid in de y-richting. Zo niet, dan is er een gewenste positieve snelheid in de y-richting.
- vx = 0.0; // Deze gewenste snelheid wordt doorgevoerd in de functie Controlling_system, die onder andere inverse kinematics en een
+ vx = 0.0; // Deze gewenste snelheid wordt doorgevoerd in de functie Controlling_system, die onder andere inverse kinematics en een
vy = 0.02; // PI-controller bevat. Uiteindelijk wordt aan beide motoren een snelheid en richting toegekend, zodat de robot met de
- } // gewenste snelheid in een rechte lijn verticaal kan bewegen.
+ } // gewenste snelheid in een rechte lijn verticaal kan bewegen.
if (normalized_EMG_calf >= 0.3) {
vx = 0.0;
vy = -0.02;
@@ -541,7 +546,7 @@
Controlling_system();
- } else if (normalized_EMG_biceps_left >= 0.3) { // Indien de rechter biceps niet wordt aangespannen en de genormaliseerde EMG-waarde van de linker biceps groter is dan 0.3, geldt
+ } else if (normalized_EMG_biceps_left >= 0.3) { // Indien de rechter biceps niet wordt aangespannen en de genormaliseerde EMG-waarde van de linker biceps groter is dan 0.3, geldt
if (normalized_EMG_calf < 0.3) { // hetzelfde als hierboven beschreven, maar dan voor de x-richting. Dit resulteert in een horizontale beweging in een rechte lijn.
vx = 0.02;
vy = 0.0;
@@ -563,12 +568,12 @@
break;
default:
-
+
motors_off(); // Hier wordt dezelfde functie als eerder toegepast om de motoren uit te schakelen -> safety!
pc.printf("Unknown or uninplemented state reached!\r\n");
}
- loop_done = true; // Dit is nog een element voor het testen of de ProcessStateMachine zichzelf inhaalt. Indien de volledige
+ loop_done = true; // Dit is nog een element voor het testen of de ProcessStateMachine zichzelf inhaalt. Indien de volledige
} // loop wel doorlopen is, verandert de bool hier in true. Hierdoor wordt de string aan het begin van de state machine
// NIET geprint. Zie een eerdere comment (vlak voor de definitie van de state machine) voor verdere toelichting.
int main(void)
@@ -580,9 +585,9 @@
pc.printf("Opstarten\r\n");
- // Chain voor rechter biceps // De filterketens zijn apart voor elke spier gedefinieerd. Er wordt eerst een keten aangemaakt die bestaat uit een
+ // Chain voor rechter biceps // De filterketens zijn apart voor elke spier gedefinieerd. Er wordt eerst een keten aangemaakt die bestaat uit een
bqcbr.add(&bqbr1).add(&bqbr2); // high-pass filter, gevolgd door een notch filter. Na het nemen van de absolute waarde van het bewerkte EMG-signaal
- bqcbr2.add(&bqbr3).add(&bqbr4); // moet het tweede gedeelte van de filterketen worden toegepast. Hiervoor moeten twee low-pass filters aan elkaar
+ bqcbr2.add(&bqbr3).add(&bqbr4); // moet het tweede gedeelte van de filterketen worden toegepast. Hiervoor moeten twee low-pass filters aan elkaar
// Chain voor linker biceps // geketend worden.
bqcbl.add(&bqbl1).add(&bqbl2);
bqcbl2.add(&bqbl3).add(&bqbl4);