Engine control program with 3 engines Needs to make a case for each simultaneous engine setting, because of the WAIT after each number of bits have been sent.
Dependencies: mbed-rtos mbed PinDetect
Fork of FinalMotorControl by
Diff: main.cpp
- Revision:
- 27:35c962e8e95b
- Parent:
- 26:57c62b925064
- Child:
- 28:dac262b7ab32
diff -r 57c62b925064 -r 35c962e8e95b main.cpp --- a/main.cpp Mon May 30 09:15:51 2016 +0000 +++ b/main.cpp Wed Jun 01 17:40:54 2016 +0000 @@ -34,26 +34,67 @@ DigitalOut IN12(D6); -static int speed = 5000; +static int speed = 1750; static int bootStep1 = 100; static int bootStep2 = 100; static int bootStep3 = 100; static char m_cmd = 'x'; +static char cmd; static bool e1 = true; static bool e2 = true; static bool e3 = true; static bool rest = false; -static bool sync1 = false; -static bool sync2 = false; -static bool sync3 = false; - static int steps1 = 0; static int steps2 = 0; static int steps3 = 0; //Methods to set pins to control direction. + +void step2Left3Right() +{ + //engine 2 + IN5=1; + IN6=0; + IN7=0; + IN8=1; + //engine 3 + IN9=0; + IN10=1; + IN11=0; + IN12=1; + + wait_us(speed); + IN5=1; + IN6=0; + IN7=1; + IN8=0; + IN9=0; + IN10=1; + IN11=1; + IN12=0; + wait_us(speed); + IN5=0; + IN6=1; + IN7=1; + IN8=0; + IN9=1; + IN10=0; + IN11=1; + IN12=0; + wait_us(speed); + IN5=0; + IN6=1; + IN7=0; + IN8=1; + IN9=1; + IN10=0; + IN11=0; + IN12=1; + wait_us(speed); +} + void stepAllRight() { //engine 1 @@ -339,31 +380,6 @@ wait_us(speed); } - -void step1() -{ - //engine 1 - IN1=1; - IN2=0; - IN3=0; - IN4=1; -} - -void step2() -{ - //engine 1 - IN1=0; - IN2=1; - IN3=0; - IN4=1; -} - -void wait() -{ - wait_us(speed); - -} - //Method to release all engines. void stopAll() { @@ -407,10 +423,21 @@ } - - - - +void hold() +{ + IN1=1; + IN2=0; + IN3=0; + IN4=1; + IN5=1; + IN6=0; + IN7=0; + IN8=1; + IN9=1; + IN10=0; + IN11=0; + IN12=1; +} void input(void const *args) @@ -419,47 +446,55 @@ { if(pc.readable()) - { m_cmd=pc.getc(); - - } - + { + m_cmd=pc.getc(); + } Thread::wait(100); //pc.printf("%d", steps); + /* + if (m_cmd == 'x') + { + while(speed < 7000) + { + //Thread::wait(1); + speed ++; + } + speed = 2000; + cmd = 'x'; + break; + } + */ + + } } -//MOTOR 1 RIGHT SIDE BUTTON +//MOTOR 1 LEFT SIDE BUTTON void pb1_hit_callback (void) { if(rest == false) { e1 = false; - //speed = 2000; - if (sync1 == true) + speed = 2000; + Thread::wait(1000); + int stepsToCenter = 242; + + while (stepsToCenter >= 0) { - Thread::wait(1000); - - int stepsToCenter = steps1/2; - - while (stepsToCenter >= 0) - { - stepEngine1Left(); - stepsToCenter --; - } - //break; - + stepEngine1Right(); + stepsToCenter --; } + steps1 = 0; + //break; + //end if + m_cmd = 'x'; } - - - } - -//MOTOR 1 LEFT SIDE BUTTON +//MOTOR 1 RIGHT SIDE BUTTON void pb2_hit_callback (void) { if(rest == false) @@ -467,20 +502,18 @@ e1 = false; //m_cmd = 'x'; - //speed = 2000; - if (sync1 == true) + speed = 2000; + Thread::wait(1000); + int stepsToCenter = 242; + + while (stepsToCenter >= 0) { - Thread::wait(1000); - int stepsToCenter = 242; - - while (stepsToCenter >= 0) - { - stepEngine1Right(); - stepsToCenter --; - } + stepEngine1Left(); + stepsToCenter --; } + steps1 = 0; + m_cmd = 'x'; } - } //MOTOR 2 LEFT SIDE BUTTON void pb3_hit_callback (void) @@ -495,9 +528,11 @@ while (stepsToCenter >= 0) { - stepEngine2Left(); + stepEngine2Right(); stepsToCenter --; } + steps2 = 0; + m_cmd = 'x'; } } @@ -515,9 +550,11 @@ while (stepsToCenter >= 0) { - stepEngine2Right(); + stepEngine2Left(); stepsToCenter --; } + steps2 = 0; + m_cmd = 'x'; } @@ -530,15 +567,17 @@ { e3 = false; //m_cmd = 'x'; - speed = 2000; + speed = 2500; Thread::wait(1000); - int stepsToCenter = 242; + int stepsToCenter = 200; while (stepsToCenter >= 0) { stepEngine3Left(); stepsToCenter --; } + steps3 = 0; + m_cmd = 'x'; } @@ -546,24 +585,29 @@ //MOTOR 3 RIGHT SIDE BUTTON void pb6_hit_callback (void) { + if(rest == false) { e3 = false; //m_cmd = 'x'; speed = 2000; Thread::wait(1000); - int stepsToCenter = 242; + int stepsToCenter = 280; while (stepsToCenter >= 0) { stepEngine3Right(); stepsToCenter --; } + //reset steps and stop engine + steps3 = 0; + m_cmd = 'x'; } } int main() { + int slowDown; pb1.mode(PullUp); pb2.mode(PullUp); @@ -594,21 +638,8 @@ stepEngine1Right(); } stepEngine1Left(); - //Found corner! if (e1 == false) { - e1= true; - Thread::wait(1000); - while(true) - { - steps1++; - //Thread::wait(100); - pc.printf("%d", steps1); - stepEngine1Right(); - if(e1==false) - break; - } - Thread::wait(1000); break; }//end if @@ -635,6 +666,7 @@ stepEngine2Left(); if (e2 == false) { + Thread::wait(1000); break; }//end if @@ -646,7 +678,7 @@ pb6.setSampleFrequency(); while (true) { - speed = 2000; + //speed = 2000; while(bootStep3 >= 0) { bootStep3--; @@ -656,7 +688,7 @@ stepEngine3Left(); if (e3 == false) { - pc.printf("HALLO"); + //pc.printf("HALLO"); Thread::wait(1000); break; }//end if @@ -664,17 +696,93 @@ while (true) { - speed = 1500; - + //speed = 1500; + switch (m_cmd) + { + case '7': + cmd = '7'; + break; + + case '9': + cmd = '9'; + break; + + case '4': + cmd= '4'; + break; + + case '6': + cmd = '6'; + break; + + case '1': + cmd = '1'; + break; + + case '3': + cmd = '3'; + break; + + case 'x': + cmd = 'x'; + break; + + //release motors + case 'z': + + //step all motors to the middle + + while(steps1 > 0) + { + stepEngine1Left(); + steps1--; + } + while(steps1 < 0) + { + stepEngine1Right(); + steps1++; + } + + while(steps2 > 0) + { + stepEngine2Left(); + steps2--; + } + while(steps2 < 0) + { + stepEngine2Right(); + steps2++; + } + + while(steps3 > 0) + { + stepEngine3Left(); + steps3--; + } + while(steps3 < 0) + { + stepEngine3Right(); + steps3++; + } + + cmd = 'z'; + + + break; + + } + //Release all motors / Set all pins to 0 - if (m_cmd == 'z') + if (cmd == 'z') { stopAll(); } //Make all engines halt and hold still. - if (m_cmd == 'x') + if (cmd == 'x') { + hold(); + } //Rest mode. Make the engines ignore pushbuttons. @@ -684,52 +792,96 @@ rest = true; //Step motors to rest position - } */ //Controlling each motor seperately. //ENGINE 3 - if(m_cmd == '7' && steps3 > -242) + if(cmd == '7' && steps3 > -200) { rest = false; steps3 --; stepEngine3Left(); + } - else if (m_cmd == '9' && steps3 < 242) + else if (cmd == '9' && steps3 < 280) { rest = false; stepEngine3Right(); steps3 ++; } //ENGINE 2 - else if (m_cmd == '4' && steps2 > -242) + else if (cmd == '4' && steps2 > -242) { rest = false; steps2 --; stepEngine2Left(); } - else if (m_cmd == '6' && steps2 < 242) + else if (cmd == '6' && steps2 < 242) { rest = false; steps2 ++; stepEngine2Right(); } //ENGINE 3 - else if (m_cmd == '1' && steps1 > -242) + else if (cmd == '1' && steps1 > -242) { rest = false; steps1--; stepEngine1Left(); } - else if (m_cmd == '3' && steps1 < 242) + else if (cmd == '3' && steps1 < 242) { rest = false; steps1++; stepEngine1Right(); } + if (m_cmd == 'y') + { + int step = 100; + while (step >= 0) + { + step--; + step2Left3Right(); + } + m_cmd = 'x'; + + } + + if (m_cmd == 't') + { + int step1, step2, step3; + step1 = 100; + step2 = 60; + step3 = 70; + + while (step1 > 0) + { + stepEngine1Left(); + step1--; + //global stepcounter + steps1--; + } + while(step2 > 0) + { + stepEngine2Right(); + step2--; + //global stepcounter + steps2++; + } + while(step3 > 0) + { + stepEngine3Left(); + step3--; + //global stepcounter + steps3--; + } + + m_cmd = 'x'; + + } @@ -740,16 +892,3 @@ } //END WHILE TRUE } //END Main - - - - - - - - - - - - -