Remco Dasselaar
/
TotalControlEmg2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI TextLCD mbed
Fork of
TotalControlEmg2
by 
Remco Dasselaar
Diff: main.cpp
- Revision:
- 33:b4757132437e
- Parent:
- 32:40691708c68c
- Child:
- 34:60391fb72629
diff -r 40691708c68c -r b4757132437e main.cpp
--- a/main.cpp Mon Oct 19 11:24:44 2015 +0000
+++ b/main.cpp Mon Oct 19 20:14:13 2015 +0000
@@ -1,3 +1,4 @@
+//--------------------Include files and libraries-------
#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
@@ -7,105 +8,100 @@
#include "Kalibratie.h"
#include "Mode.h"
//--------------------Classes------------------------
-InterruptIn btnSet(PTC6); // kalibreer knop
-DigitalOut ledR(LED_RED), LedB(LED3); // Led op moederbord
+InterruptIn btnSet(PTC6); // Kalibration button
+DigitalOut ledR(LED_RED), LedB(LED3); // Leds on K64F
MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty
-TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD scherm op binnenste rij van K64F, rs, e, d4-d7
+TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD screen on inner row of K64F, rs, e, d4-d7
PwmOut servo(D3); // PwmOut servo
-AnalogIn emgL(A0), emgR(A1); // Analog input van emg kabels links en rechts
-AnalogIn potL(A2), potR(A3); // Intensiteit 'EMG' signaal door potmeter
+AnalogIn emgL(A0), emgR(A1); // Analog input of EMG, left and right
+AnalogIn potL(A2), potR(A3); // Potential meter left and right
AnalogIn KS(A4); // Killswitch
-HIDScope scope(6);
-Ticker EMGticker, tickerRegelaar, tickerRem; // regelaar button en rem ticker
-
-// QEI Encoder(poort 1, poort 2, ,counts/rev
+HIDScope scope(6); // Hidscope, amount of scopes
+Ticker EMGticker, tickerControl, tickerBreak; // Ticker for EMG, regulator and break
+// QEI Encoder(port 1, port 2, ,counts/rev
QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
-// Motor1 met PWM aansturen en richting aangeven
+// Motor1 met PWM power control and direction
PwmOut pwmM1(D6);
DigitalOut dirM1(D7);
-// Motor2 met PWM aansturen en richting aangeven
+// Motor2 met PWM power control and direction
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
-enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE}; // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
-uint8_t state = KALIBRATE_EMG; // eerste spelfase
-enum aimFase {OFF, CW, CCW}; // Aim motor mogelijkheden
-uint8_t aimState = OFF; // mogelijkheid begin
+enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, ACKNOWLEDGEMENT switch: BMT groep 4 2014
+uint8_t state = KALIBRATE_EMG; // first state program
+enum aimFase {OFF, CW, CCW}; // Aim motor possible states
+uint8_t aimState = OFF; // first state aim motor
//-------------------------------Variables---------------------------------------------------------------------
-const int on = 0, off = 1; // aan / uit
-int maxCounts = 2100; // max richt motor counts Aim motor
-int remPerc = 0;
-
-const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R (= pulsewidth pwm servo)
-const double tRegelaar = 0.005, tRem = 0.1; // tijd ticker voor regelaar en knoppen/EMG
-const double Fs = 50; //Sample frequentie Hz
-double t = 1/ Fs; // tijd EMGticker
+const int on = 0, off = 1; // on off
+int maxCounts = 42000; // max richt motor counts Aim motor
+int breakPerc = 0;
+const double servoL = 0.001, servoR = 0.0011; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
+const double tControl = 0.005, tBreak = 0.1; // ticker for motor checking
+const double Fs = 50; //Sample frequency Hz
+double t = 1/ Fs; // time EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
-double yL = 0, yR = 0; // y waarden EMG links en rechts
-
-volatile bool L = false, R = false, RH = false; // Booleans voor knop link en knop rechts
-volatile bool btn = false; // boolean om programma te runnen, drukknop ingedrukt
-volatile bool regelaarFlag = false, btnFlag = false, remFlag = false; // Go flags
-volatile bool emgFlag = false;
+double yL = 0, yR = 0; // y values EMG left and right
+volatile bool L = false, R = false; // Booleans for checking if mode. has been 1?
+volatile bool btn = false; // Button is pressed?
+volatile bool controlFlag = false, btnFlag = false, breakFlag = false, emgFlag = false; // Go flags
//----------------------------Functions-----------------------------------------------------------------------
void flipLed(DigitalOut& led){ //function to flip one LED
led.write(on);
wait(0.2);
led.write(off);
}
-void PRINT(const char* text){ // putty en lcd print
+void PRINT(const char* text){
lcd.cls(); // clear LCD scherm
- lcd.printf(text); // print text op lcd
- pc.printf(text); // print text op terminal
+ lcd.printf(text); // print text op lcd
}
-void EMGkalibratieL(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
+void EMGkalibratieL(){ // Determine thresholds left
LedB = 1;
Init();
double ymin = KalibratieMin(emgL, true);
wait(1);
double ymax = KalibratieMax(emgL, true);
- if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
+ if((ymax-ymin) < 0.05){ // No EMG connected
ymin = 10;
ymax = 10;
}
- thresholdlowL = 4 * ymin; // standaardwaarde
- thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
- thresholdhighL = 0.8 * ymax;
+ thresholdlowL = 4 * ymin; // Lowest threshold
+ thresholdmidL = 0.5 * ymax; // Midi threshold
+ thresholdhighL = 0.8 * ymax; // Highest threshold
- pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); //bugfix
+ pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
}
-void EMGkalibratieR(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
+void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
LedB = 1;
Init();
double ymin = KalibratieMin(emgR, false);
wait(1);
double ymax = KalibratieMax(emgR, false);
- if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
+ if((ymax-ymin) < 0.05){
ymin = 10;
ymax = 10;
}
- thresholdlowR = 4 * ymin; // standaardwaarde
- thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
+ thresholdlowR = 4 * ymin;
+ thresholdmidR = 0.5 * ymax;
thresholdhighR = 0.8 * ymax;
- pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
+ pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal
}
int EMGfilter(AnalogIn& emg, bool side){
- double u = emg.read(); // uitlezen emg signaal (linker of rechter EMG)
+ double u = emg.read(); // read emg signal (left or right EMG)
int mode = 1;
if(side){
- double y = FilterdesignsLeft(u); // signaal filteren // linker EMG
- mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
+ double y = FilterdesignsLeft(u); // filter signal left EMG
+ mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3)
}
else {
- double y = FilterdesignsRight(u); // signaal filteren // rechter EMG
- mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
+ double y = FilterdesignsRight(u);
+ mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); // right EMG
}
return mode;
}
-int PotReader(AnalogIn& pot){ // potmeter uitlezen en mode bepalen (thresholds)
+int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
double volt = pot.read();
int mode = 1;
if(volt > 0.8){
@@ -116,12 +112,12 @@
}
return mode;
}
-int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // bepaal mode uit emg en pot
+int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // Determine mode both from EMG and Potentialmeter, ONE of them must be ONE!
int emgMode = EMGfilter(emg, side);
int potMode = PotReader(pot);
int mode = 1;
- if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
- if(emgMode > potMode){ // maximum van emg en pot
+ if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
+ if(emgMode > potMode){ // maximum of emg and pot
mode = emgMode;
}
else{
@@ -130,138 +126,138 @@
}
return mode;
}
-void setEmgFlag(){
+void setEmgFlag(){ // Goflag EMG
emgFlag = true;
}
void btnSetAction(){ // Set knop K64F
btn = true; // GoFlag setknop
}
-void setRegelaarFlag(){ // Go flag button controle
- regelaarFlag = true;
+void setControlFlag(){ // Go flag setButton
+ controlFlag = true;
}
-void setRemFlag(){ // Go flag rem
- remFlag = true;
+void setBreakFlag(){ // Go flag Break
+ breakFlag = true;
}
-void checkAim(){ // controleer of killer switch geraakt is en of max aantal counts niet bereikt is
- /*double volt = KS.read();
- if(volt> 0.5 /*|| abs(enc2.getPulses()) > maxCounts*///){
- /*pwmM2.write(0); // Aim motor stilzetten
- pc.printf("BOEM! CRASH! KASTUK! \r\n");
- }*/
+void checkAim(){ // check if Killswitch is on or max counts is reached
+ double volt = KS.read();
+ if(volt> 0.5 || abs(enc2.getPulses()) > maxCounts*/){
+ pwmM2.write(0); // Aim motor freeze
+ pc.printf("BOEM! CRASH! KASTUK! \r\n"); // Terminal
+ PRINT("BOEM! CRASH!"); // LCD
+ }
}
-void motorAim(int dir){ // Aim motor laten draaien met gegeven direction
- dirM2.write(dir); // richting
- pwmM2.write(0.25); // width aanpassen
+void motorAim(int dir){ // Rotate Aim motor with given direction
+ dirM2.write(dir);
+ pwmM2.write(0.25);
}
-bool controlAim(){ // Function om aim motor aan te sturen
- bool both = false; // boolean of beide knoppen ingedrukt zijn
-
+bool controlAim(){ // Function to control aim motor with modes
+ bool both = false; // boolean if both modes are 3
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
scope.set(0, modeL);
scope.set(1, modeR);
- scope.send(); //stuur de waardes naar HIDscope
+ scope.send(); //send values to HIDScope
- if(modeR < 2 && !R){ // controleer of mode 1 is geweest
+ if(modeR < 2 && !R){ // control if mode has been 1
R = true;
}
if(modeL < 2 && !L){
L = true;
}
- if((modeL>2) && (modeR >2 && R && L)) { // mode L en mode R beide > 2
+ if((modeL>2) && (modeR >2 && R && L)) { // mode L and mode R both 3, and both has been 1 herefore
both = true; // Return true
- pwmM2.write(0); // Aim motor stilzetten
- aimState = OFF;
+ pwmM2.write(0); // Aim motor freeze
+ aimState = OFF; // next state
}
- else if((modeR == 2) && (modeL == 2)) {
- if(aimState!=OFF){
- pwmM2.write(0); // Aim motor stilzetten
- aimState = OFF;
- PRINT("Motor freeze\r\n");
- L = false;
- R = false;
+ else if((modeR == 2) && (modeL == 2)) { // modes are both 2
+ if(aimState!=OFF){ // only if aim motor is rotating
+ pwmM2.write(0); // Aim motor freeze
+ aimState = OFF; // motor state is off
+ PRINT("Motor freeze\r\n"); // LCD
+ L = false; // Modes must be first 1 for another action
+ R = false; // ""
}
}
- else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND richting is niet CCW AND modeR = 1
+ else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
if(L){
- aimState = CCW; // draai CCW
- pc.printf("Turn negative (CCW)\r\n");
+ aimState = CCW; // Rotate CCW
+ pc.printf("Rotate -, CCW");
motorAim(0);
}
}
- else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND richting is niet CW AND modeL = 1
+ else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
if(R){
- aimState = CW; // draai CW
- PRINT("Turn positive (CW)\r\n");
+ aimState = CW; // Rotate CW
+ PRINT("Rotate +, CW");
motorAim(1);
}
}
return both;
}
int main(){
- flipLed(ledR); // test of code begint
+ flipLed(ledR); // test if code begins
btnSet.mode(PullUp); // Button mode
- btnSet.rise(&btnSetAction); // Setknop aan functie koppellen
- tickerRegelaar.attach(&setRegelaarFlag,tRegelaar); // ticker regelaar motor
- tickerRem.attach(&setRemFlag,tRem); // ticker rem
+ btnSet.rise(&btnSetAction); // Connect button to function
+ tickerControl.attach(&setControlFlag,tControl); // ticker control motor
+ tickerBreak.attach(&setBreakFlag,tBreak); // ticker break
EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
- pc.printf("\n\n\n\n\n");
- PRINT("--- NEW GAME ---\r\n");
- while(1){ // Programma door laten lopen
+ pc.printf("\n\n\n\n\n"); // Terminal
+ pc.prinft("---NEW GAME---\r\n)
+ PRINT("--- NEW GAME ---"); // LCD
+ while(1){ // Run program
switch(state){
case KALIBRATE_EMG: {
- PRINT("Kalibrate EMG left in 5 seconds\r\n");
-
- EMGkalibratieL();
+ pc.printf("Kalibrate EMG left in 5 seconds\r\n"); // Terminal
+ PRINT("EMG LEFT MAX MIN"); // LCD
+ EMGkalibratieL(); // Kalibrate left EMG, determine thresholds
- PRINT("Kalibrate EMG right in 5 seconds\r\n");
- EMGkalibratieR();
+ pc.printf("Kalibrate EMG right in 5 seconds\r\n"); // Terminal
+ PRINT("EMG RIGHT MAX MIN"); // LCD
+ EMGkalibratieR(); // Kalibrate right EMG, determine thresholds
- state = KALIBRATE_AIM;
+ state = KALIBRATE_AIM; // Next state
break;
}
case KALIBRATE_AIM: {
- pwmM2.period(1/100000); // aanstuurperiode motor 2
- PRINT("Position is kalibrating\r\n");
- wait(1);
- //pc.printf("Use L and R to move pendulum to equilibruim position\r\n L+R = OK\r\n");
- while(state==KALIBRATE_AIM){
- dirM2.write(0); // richting
- pwmM2.write(0.25); // width aanpassen
-
- if(KS.read() > 0.5){
- pwmM2.write(0); // motor stilzetten
- enc2.reset(); // resetknop = encoder 1 resetten
- PRINT("Position kalibrated\r\n");
- state = KALIBRATE_PEND; // volgende state
+ pwmM2.period(1/100000); // period motor 2
+ printf("Position is kalibrating\r\n"); // terminal
+ PRINT("Wait a moment, please"); // LCD
+ dirM2.write(0); // direction aim motor
+ pwmM2.write(0.25); // percentage motor power
+ bool kalibrated = false;
+ while(state==KALIBRATE_AIM){
+ pc.printf("Killswitch: %f\r\n", KS.read()); // terminal
+ if(KS.read() > 0.5){ // Killswitch?
+ pwmM2.write(0); // Aim motor freeze
+ enc2.reset(); // Reset encoder
+ PRINT("Aim kalibrated");
+ //state = KALIBRATE_PEND; // next state
+ kalibrated = true;
+ dirM2.write(0); // direction aim motor
+ pwmM2.write(0.25); // percentage motor power, turn it on
}
-
- /*
- if(regelaarFlag){ // motor regelen op GoFlag
- regelaarFlag = false;
- checkAim(); // Controleer positie
- }
- if(emgFlag){ // Go flag EMG sampelen
- emgFlag = false;
- if(controlAim()){ // Buttons met control, if true = beide knoppen = bevestigen
- enc2.reset(); // resetknop = encoder 1 resetten
- pc.printf("Positie gekalibreerd, kalibreer nu slinger, encoder counts: %i\r\n", enc2.getPulses());
- state = KALIBRATE_PEND; // volgende state
- }
- }*/
+ if(controlFlag && kalibrated){ // motor regelen op GoFlag
+ controlFlag = false;
+ if(enc.getPulses() > maxCounts/2){ // rotate aim motor to midi position
+ pwmM2.write(0); // Aim motor freeze
+ state = KALIBRATE_PEND; // next state
+ }
+ }
}
break;
}
case KALIBRATE_PEND: {
pc.printf("Kalibrate pendulum motor with setknop\r\n");
- pwmM1.period(1/100000); // aanstuurperiode motor 1
- servo.period(0.02); // 20ms period aanstuurperiode
- pwmM1.write(0.25); // Motor aanzetten, laag vermogen
- btn = false;
+ pwmM1.period(1/100000); // periode pendulum motor
+ servo.period(0.02); // 20ms period servo
+ pwmM1.write(0.25); // Turn motor on, low power
+ btn = false;
+ R = false;
+ L = false;
while(state==KALIBRATE_PEND){
if(emgFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
@@ -269,20 +265,27 @@
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
+
+ if(modeR < 2) { // modeR == 1
+ R = true;
+ }
+ if(modeL < 2) { // modeL == 1
+ L = true;
+ }
- if (btn || (modeL == 3) || (modeR == 3)){ // Als setknop ingedrukt is reset
- pwmM1.write(0); // Motor 1 stilzetten
- enc1.reset(); // encoder 1 resetten
+ if (btn || (modeL == 3 && L) || (modeR == 3 && R)){ // If setbutton is on or one mode is 3, and has been 1
+ pwmM1.write(0); // Motor 1 freeze
+ enc1.reset(); // encoder 1 reset
PRINT("Pendulum kalibrated\r\n");
- btn = false; // knop op false
- state = AIM; // volgende fase
+ btn = false; // button op false
+ state = AIM; // next state
}
else if(modeL == 2){
- pwmM1.write(0); // Pendulum stilzetten
+ pwmM1.write(0); // Pendulum freeze
PRINT("Pendulum off\r\n");
}
else if(modeL == 3){
- pwmM1.write(0.025); // Pendulum aanzetten
+ pwmM1.write(0.025); // Pendulum rotate
PRINT("Pendulum on\r\n");
}
}
@@ -292,92 +295,93 @@
case AIM: {
pc.printf("Aim with EMG\r\n");
while(state == AIM){
- if(regelaarFlag){ // motor regelen op GoFlag
- regelaarFlag = false;
- checkAim(); // Controleer positie
+ if(controlFlag){ // motor control on GoFlag
+ controlFlag = false;
+ checkAim(); // Check position
}
- if(emgFlag){ // Go flag EMG sampelen
+ if(emgFlag){ // Go flag EMG sampel
emgFlag = false;
if(controlAim()){
pc.printf("Position choosen, adjust shoot velocity\r\n");
- state = REM; // volgende state (REM)
+ state = REM; // Next state (BREAK)
}
}
}
break;
}
- case REM: {
- pc.printf("Set break percentage, current is: %.0f\r\n", remPerc);
+ case BREAK: {
+ pc.printf("Set break percentage, current is: %.0f\r\n", breakPerc);
L = false;
R = false;
- while(state == REM){
+ while(state == BREAK){
if(emgFlag){ // Go flag EMG sampelen
emgFlag = false;
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
- if(modeR < 2) { // R is niet aan
+ if(modeR < 2) { // modeR is 1
R = true;
}
- if(modeL < 2) { // L is niet aan
+ if(modeL < 2) { // modeL is 1
L = true;
}
if(L && R){
- if((modeL > 1) && modeR > 1) { // beide aangespannenR = false;
+ if((modeL > 1) && modeR > 1) { // both modes
state = FIRE;
R = false;
L = false;
}
- else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten
+ else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
state = FIRE;
R = false;
L = false;
}
- else if((modeL == 2) && L) { // links ingedrukt = rem verlagen
- if(remPerc>1){
- remPerc--; // Rem percentage verlagen met 1
+ else if((modeL == 2) && L) { // modeL = BREAK lower with one
+ if(breakPerc>1){
+ breakPerc--;
}
L = false;
}
- else if((modeR == 2) && R) { // Rechts half ingedrukt = rem verhogen
- if(remPerc<10){
- remPerc++; // rem percentage verhogen met 1
+ else if((modeR == 2) && R) { // modeR = Break +1
+ if(breakPerc<10){
+ breakPerc++;
}
R = false;
}
- if(modeL > 1 || modeR > 1){ // Print rem scale als een of beide knoppen aan zijn
- pc.printf("Current breaking scale: %i\r\n", remPerc);
+ if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
+ pc.printf("Current breaking scale: %i\r\n", breakPerc);
+ PRINT("Break scale is: %i", breakPerc);
}
}
}
}
- L = false;
- R = false;
+ L = false; // modeL must be one for another action can take place
+ R = false; // modeR ""
break;
}
case FIRE: {
pc.printf("Shooting\r\n");
- servo.pulsewidth(servoL); // schrijfrem release
- pwmM1.write(0.25); // motor aanzetten
+ servo.pulsewidth(servoL); // BREAK release
+ pwmM1.write(0.25); // Pendulum motor on
bool servoPos = true;
- while(state == FIRE){ // Zolang in FIRE state
- if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(remPerc)/10)){ // Rem goFlag en half rondje = remmen
- remFlag = false;
- if(servoPos){ //
+ while(state == FIRE){ // while in FIRE state
+ if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){ // BREAK goFlag half rotation pendulum = breaking
+ breakFlag = false;
+ if(servoPos){
servoPos = false;
- servo.pulsewidth(servoL); // links
+ servo.pulsewidth(servoL); // left
}
else{
servoPos = true;
- servo.pulsewidth(servoR); // rechts
+ servo.pulsewidth(servoR); // right
}
}
- if(regelaarFlag){
- regelaarFlag = false;
- if((abs(enc1.getPulses())+50)%4200 < 150){ // Slinger 1 rondje laten draaien
- pwmM1.write(0); // motor uitzetten
- PRINT("Pendulum on startposition\r\n");
- state = AIM; // Aim is volgende fase
+ if(controlFlag){
+ controlFlag = false;
+ if((abs(enc1.getPulses())+50)%4200 < 150){ // rotate pendulum one revolution
+ pwmM1.write(0); // motor pendulum off
+ pc.printf("Pendulum on startposition\r\n");
+ state = AIM; // Next stage
}
}
}