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
main.cpp
- Committer:
- mjhaugsdal
- Date:
- 2016-07-07
- Revision:
- 29:00f839aad30e
- Parent:
- 28:dac262b7ab32
- Child:
- 30:fac5b5624fe3
File content as of revision 29:00f839aad30e:
#include "mbed.h" #include "rtos.h" #include "PinDetect.h" Serial pc(USBTX, USBRX); // tx, rx //Mid PinDetect pb3(A4); PinDetect pb4(A5); //Bottom PinDetect pb1(D14); PinDetect pb2(D15); //Top PinDetect pb5(D12); PinDetect pb6(D13); //Analog Pins //First engine DigitalOut IN1(A0); DigitalOut IN2(A1); DigitalOut IN3(A2); DigitalOut IN4(A3); //Second engine DigitalOut IN5(D8); DigitalOut IN6(D9); DigitalOut IN7(D10); DigitalOut IN8(D11); //Third Engine DigitalOut IN9(D3); DigitalOut IN10(D4); DigitalOut IN11(D5); DigitalOut IN12(D6); //User button DigitalIn SW(USER_BUTTON); 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 int i = 0; 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 IN1=0; IN2=1; IN3=0; IN4=1; //engine 2 IN5=0; IN6=1; IN7=0; IN8=1; //engine 3 IN9=0; IN10=1; IN11=0; IN12=1; wait_us(speed); //legg som global variabel "fart" //engine 1 IN1=0; IN2=1; IN3=1; IN4=0; //engine 2 IN5=0; IN6=1; IN7=1; IN8=0; //engine 3 IN9=0; IN10=1; IN11=1; IN12=0; wait_us(speed); //engine 1 IN1=1; IN2=0; IN3=1; IN4=0; //engine 2 IN5=1; IN6=0; IN7=1; IN8=0; //engine 3 IN9=1; IN10=0; IN11=1; IN12=0; wait_us(speed); //engine 1 IN1=1; IN2=0; IN3=0; IN4=1; //engine 2 IN5=1; IN6=0; IN7=0; IN8=1; //engine 3 IN9=1; IN10=0; IN11=0; IN12=1; wait_us(speed); } void stepAllLeft() { //engine 1 IN1=1; IN2=0; IN3=0; IN4=1; //engine 2 IN5=1; IN6=0; IN7=0; IN8=1; //engine 3 IN9=1; IN10=0; IN11=0; IN12=1; wait_us(speed); //engine 1 IN1=1; IN2=0; IN3=1; IN4=0; //engine 2 IN5=1; IN6=0; IN7=1; IN8=0; //engine 3 IN9=1; IN10=0; IN11=1; IN12=0; wait_us(speed); //engine 1 IN1=0; IN2=1; IN3=1; IN4=0; //engine 2 IN5=0; IN6=1; IN7=1; IN8=0; //engine 3 IN9=0; IN10=1; IN11=1; IN12=0; wait_us(speed); //engine 1 IN1=0; IN2=1; IN3=0; IN4=1; //engine 2 IN5=0; IN6=1; IN7=0; IN8=1; //engine 3 IN9=0; IN10=1; IN11=0; IN12=1; wait_us(speed); } void stepEngine1Left() { //engine 1 IN1=1; IN2=0; IN3=0; IN4=1; wait_us(speed); IN1=1; IN2=0; IN3=1; IN4=0; wait_us(speed); IN1=0; IN2=1; IN3=1; IN4=0; wait_us(speed); IN1=0; IN2=1; IN3=0; IN4=1; wait_us(speed); } void stepEngine1Right() { //engine 1 IN1=0; IN2=1; IN3=0; IN4=1; wait_us(speed); IN1=0; IN2=1; IN3=1; IN4=0; wait_us(speed); IN1=1; IN2=0; IN3=1; IN4=0; wait_us(speed); IN1=1; IN2=0; IN3=0; IN4=1; wait_us(speed); } void stepEngine2Right() { //engine 1 IN5=0; IN6=1; IN7=0; IN8=1; wait_us(speed); IN5=0; IN6=1; IN7=1; IN8=0; wait_us(speed); IN5=1; IN6=0; IN7=1; IN8=0; wait_us(speed); IN5=1; IN6=0; IN7=0; IN8=1; wait_us(speed); } void stepEngine2Left() { //engine 1 IN5=1; IN6=0; IN7=0; IN8=1; wait_us(speed); IN5=1; IN6=0; IN7=1; IN8=0; wait_us(speed); IN5=0; IN6=1; IN7=1; IN8=0; wait_us(speed); IN5=0; IN6=1; IN7=0; IN8=1; wait_us(speed); } void stepEngine3Right() { //engine 1 IN9=0; IN10=1; IN11=0; IN12=1; wait_us(speed); IN9=0; IN10=1; IN11=1; IN12=0; wait_us(speed); IN9=1; IN10=0; IN11=1; IN12=0; wait_us(speed); IN9=1; IN10=0; IN11=0; IN12=1; wait_us(speed); } void stepEngine3Left() { //engine 1 IN9=1; IN10=0; IN11=0; IN12=1; wait_us(speed); IN9=1; IN10=0; IN11=1; IN12=0; wait_us(speed); IN9=0; IN10=1; IN11=1; IN12=0; wait_us(speed); IN9=0; IN10=1; IN11=0; IN12=1; wait_us(speed); } //Method to release all engines. void stopAll() { IN1=0; IN2=0; IN3=0; IN4=0; IN5=0; IN6=0; IN7=0; IN8=0; IN9=0; IN10=0; IN11=0; IN12=0; } //Methods to release single engines. void stopE1() { IN1=0; IN2=0; IN3=0; IN4=0; } void stopE2() { IN5=0; IN6=0; IN7=0; IN8=0; } void stopE3() { IN9=0; IN10=0; IN11=0; IN12=0; } 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) { while(true) { if(pc.readable()) { 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 LEFT SIDE BUTTON void pb1_hit_callback (void) { if(rest == false) { e1 = false; speed = 1000; Thread::wait(1000); int stepsToCenter = i/2; while (stepsToCenter >= 0) { stepEngine1Right(); stepsToCenter --; } steps1 = 0; //break; //end if m_cmd = 'x'; } } //MOTOR 1 RIGHT SIDE BUTTON void pb2_hit_callback (void) { if(rest == false) { e1 = false; //m_cmd = 'x'; speed = 1000; Thread::wait(1000); int stepsToCenter = i/2; while (stepsToCenter >= 0) { stepEngine1Left(); stepsToCenter --; } steps1 = 0; m_cmd = 'x'; } } //MOTOR 2 LEFT SIDE BUTTON void pb3_hit_callback (void) { if(rest == false) { e2 = false; //m_cmd = 'x'; speed = 1000; Thread::wait(1000); int stepsToCenter = i/2; while (stepsToCenter >= 0) { stepEngine2Right(); stepsToCenter --; } steps2 = 0; m_cmd = 'x'; } } //MOTOR 2 RIGHT SIDE BUTTON void pb4_hit_callback (void) { if(rest == false) { e2 = false; //m_cmd = 'x'; speed = 1000; Thread::wait(1000); int stepsToCenter = i/2; while (stepsToCenter >= 0) { stepEngine2Left(); stepsToCenter --; } steps2 = 0; m_cmd = 'x'; } } //MOTOR 3 LEFT SIDE BUTTON void pb5_hit_callback (void) { if(rest == false) { e3 = false; //m_cmd = 'x'; speed = 1000; Thread::wait(1000); int stepsToCenter = i/2; while (stepsToCenter >= 0) { stepEngine3Left(); stepsToCenter --; } steps3 = 0; m_cmd = 'x'; } } //MOTOR 3 RIGHT SIDE BUTTON void pb6_hit_callback (void) { if(rest == false) { e3 = false; //m_cmd = 'x'; speed = 1000; Thread::wait(1000); int stepsToCenter = i/2; 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); pb3.mode(PullUp); pb4.mode(PullUp); pb5.mode(PullUp); pb6.mode(PullUp); //Set up buttons 1 and 2 pb1.attach_deasserted(&pb1_hit_callback); pb1.setSampleFrequency(); pb2.attach_deasserted(&pb2_hit_callback); pb2.setSampleFrequency(); //Thread 1 has constant feed from usb Thread t1(input); //static char global_direction; printf("Started"); // Move motor in one direction while (true) { /* while(bootStep1 >= 0) { bootStep1--; stepEngine1Right(); } */ stepEngine1Left(); if (e1 == false) { e1 = true; while(true) { stepEngine1Right(); i++; if(e1 == false) break; } Thread::wait(1000); break; }//end if }//end while //Set up buttons 3 and 4 pb3.attach_deasserted(&pb3_hit_callback); pb3.setSampleFrequency(); pb4.attach_deasserted(&pb4_hit_callback); pb4.setSampleFrequency(); while (true) { speed = 1000; /* while(bootStep2 >= 0) { bootStep2--; stepEngine2Right(); } */ stepEngine2Left(); if (e2 == false) { e2 = true; while(true) { stepEngine2Right(); i++; if(e2 == false) break; } Thread::wait(1000); break; }//end if } pb5.attach_deasserted(&pb5_hit_callback); pb5.setSampleFrequency(); pb6.attach_deasserted(&pb6_hit_callback); pb6.setSampleFrequency(); while (true) { //speed = 2000; /* while(bootStep3 >= 0) { bootStep3--; stepEngine3Right(); } */ stepEngine3Left(); if (e3 == false) { e3 = true; while(true) { stepEngine3Right(); i++; if(e3 == false) break; } //pc.printf("HALLO"); m_cmd = 'z'; Thread::wait(1000); break; }//end if } //normal runtime while (true) { //user button to turn off engines if(SW==1) { //put engines to sleep m_cmd ='z'; } //set the speed. Higher = slower speed = 1000; //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 (cmd == 'z') { stopAll(); } //Make all engines halt and hold still. if (cmd == 'x') { hold(); Thread::wait(1000); m_cmd = 'z'; } //Rest mode. Make the engines ignore pushbuttons. /* if(m_cmd == 'r') { rest = true; //Step motors to rest position } */ //Controlling each motor seperately. //ENGINE 3 if(cmd == '7' && steps3 > -200) { rest = false; steps3 --; stepEngine3Left(); } else if (cmd == '9' && steps3 < 280) { rest = false; stepEngine3Right(); steps3 ++; } //ENGINE 2 else if (cmd == '4' && steps2 > -242) { rest = false; steps2 --; stepEngine2Left(); } else if (cmd == '6' && steps2 < 242) { rest = false; steps2 ++; stepEngine2Right(); } //ENGINE 3 else if (cmd == '1' && steps1 > -242) { rest = false; steps1--; stepEngine1Left(); } 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'; } //Thread::wait(10); //pc.printf("%d", steps2); } //END WHILE TRUE } //END Main