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-05-30
- Revision:
- 26:57c62b925064
- Parent:
- 25:321b970eb3ff
- Child:
- 27:35c962e8e95b
File content as of revision 26:57c62b925064:
#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); static int speed = 5000; static int bootStep1 = 100; static int bootStep2 = 100; static int bootStep3 = 100; static char m_cmd = 'x'; 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 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); } 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() { 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 input(void const *args) { while(true) { if(pc.readable()) { m_cmd=pc.getc(); } Thread::wait(100); //pc.printf("%d", steps); } } //MOTOR 1 RIGHT SIDE BUTTON void pb1_hit_callback (void) { if(rest == false) { e1 = false; //speed = 2000; if (sync1 == true) { Thread::wait(1000); int stepsToCenter = steps1/2; while (stepsToCenter >= 0) { stepEngine1Left(); stepsToCenter --; } //break; } } } //MOTOR 1 LEFT SIDE BUTTON void pb2_hit_callback (void) { if(rest == false) { e1 = false; //m_cmd = 'x'; //speed = 2000; if (sync1 == true) { Thread::wait(1000); int stepsToCenter = 242; while (stepsToCenter >= 0) { stepEngine1Right(); stepsToCenter --; } } } } //MOTOR 2 LEFT SIDE BUTTON void pb3_hit_callback (void) { if(rest == false) { e2 = false; //m_cmd = 'x'; speed = 2000; Thread::wait(1000); int stepsToCenter = 242; while (stepsToCenter >= 0) { stepEngine2Left(); stepsToCenter --; } } } //MOTOR 2 RIGHT SIDE BUTTON void pb4_hit_callback (void) { if(rest == false) { e2 = false; //m_cmd = 'x'; speed = 2000; Thread::wait(1000); int stepsToCenter = 242; while (stepsToCenter >= 0) { stepEngine2Right(); stepsToCenter --; } } } //MOTOR 3 LEFT SIDE BUTTON void pb5_hit_callback (void) { if(rest == false) { e3 = false; //m_cmd = 'x'; speed = 2000; Thread::wait(1000); int stepsToCenter = 242; while (stepsToCenter >= 0) { stepEngine3Left(); stepsToCenter --; } } } //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; while (stepsToCenter >= 0) { stepEngine3Right(); stepsToCenter --; } } } int main() { 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) { speed = 2000; while(bootStep1 >= 0) { bootStep1--; 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 }//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 = 2000; while(bootStep2 >= 0) { bootStep2--; stepEngine2Right(); } stepEngine2Left(); if (e2 == false) { 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) { pc.printf("HALLO"); Thread::wait(1000); break; }//end if } while (true) { speed = 1500; //Release all motors / Set all pins to 0 if (m_cmd == 'z') { stopAll(); } //Make all engines halt and hold still. if (m_cmd == 'x') { } //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(m_cmd == '7' && steps3 > -242) { rest = false; steps3 --; stepEngine3Left(); } else if (m_cmd == '9' && steps3 < 242) { rest = false; stepEngine3Right(); steps3 ++; } //ENGINE 2 else if (m_cmd == '4' && steps2 > -242) { rest = false; steps2 --; stepEngine2Left(); } else if (m_cmd == '6' && steps2 < 242) { rest = false; steps2 ++; stepEngine2Right(); } //ENGINE 3 else if (m_cmd == '1' && steps1 > -242) { rest = false; steps1--; stepEngine1Left(); } else if (m_cmd == '3' && steps1 < 242) { rest = false; steps1++; stepEngine1Right(); } //Thread::wait(10); //pc.printf("%d", steps2); } //END WHILE TRUE } //END Main