Main Code
Dependencies: PinDetect Ulrasonic mbed-rtos mbed WTV020SD_Sound_Breakout_Library
Fork of MC by
main.cpp
- Committer:
- benrammok
- Date:
- 2016-09-01
- Revision:
- 35:64ed42e6f9a4
- Parent:
- 33:6f500188e36d
- Child:
- 36:3011c70a5c81
File content as of revision 35:64ed42e6f9a4:
#include "mbed.h" #include "rtos.h" #include "PinDetect.h" #include "ultrasonic.h" //Needs to be define to be able to read from the Ultrasonic sensor void dist(int); ultrasonic mu(A2, A3, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 //have updates every .1 seconds and a timeout after 1 //second, and call dist when the distance changes Serial pc(USBTX, USBRX); // tx, rx //MidMagnetSensors PinDetect pb3(D10); PinDetect pb4(D11); //BottomMagnetSensors PinDetect pb1(D12); PinDetect pb2(D13); //TopMagnetSensors PinDetect pb5(D8); PinDetect pb6(D9); //Analog Pins //Bottomengine DigitalOut IN1(A0); DigitalOut IN2(A1); DigitalOut IN3(A2); DigitalOut IN4(A3); //SecondEngine DigitalOut IN5(D2); DigitalOut IN6(D3); DigitalOut IN7(A4); DigitalOut IN8(A5); //TopEngine DigitalOut IN9(D4); DigitalOut IN10(D5); DigitalOut IN11(D6); DigitalOut IN12(D7); //User button DigitalIn SW(USER_BUTTON); static int speed = 1000; static char m_cmd = 'x'; static char cmd; static bool e1 = true; static bool e2 = true; static bool e3 = true; static int i1 = 0; static int i2 = 0; static int i3 = 0; static int steps1 = 0; static int steps2 = 0; static int steps3 = 0; //Distance stored from sensor static int sDistance = 0; static bool isRot = true; 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); //pc.printf("%c", m_cmd); } } //Tråd for ultralyd void ultraSound(void const *args) { while(true) { printf("Current Distance: %dmm\r\n", mu.getCurrentDistance()); /*if(mu.getCurrentDistance() > 1000){ while(true){ stepEngine3Right(); if(e3 == false) break; } } */ Thread::wait(200); } } //MOTOR 1 LEFT SIDE BUTTON void pb1_hit_callback (void) { e1 = false; speed = 1000; wait(2); int stepsToCenter = i1/2; while (stepsToCenter >= 0) { stepEngine1Right(); stepsToCenter --; } steps1 = 0; //break; //end if m_cmd = 'x'; } //MOTOR 1 RIGHT SIDE BUTTON void pb2_hit_callback (void) { e1 = false; //m_cmd = 'x'; speed = 1000; wait(2); int stepsToCenter = i1/2; while (stepsToCenter >= 0) { stepEngine1Left(); stepsToCenter --; } steps1 = 0; m_cmd = 'x'; } //MOTOR 2 LEFT SIDE BUTTON void pb3_hit_callback (void) { { e2 = false; //m_cmd = 'x'; speed = 1000; wait(1); int stepsToCenter = i2/2; while (stepsToCenter >= 0) { stepEngine2Left(); stepsToCenter --; } steps2 = 0; //m_cmd = 'x'; stopE2(); } } //MOTOR 2 RIGHT SIDE BUTTON void pb4_hit_callback (void) { { e2 = false; //m_cmd = 'x'; speed = 1000; wait(1); int stepsToCenter = i2/2; while (stepsToCenter >= 0) { stepEngine2Right(); stepsToCenter --; } steps2 = 0; //m_cmd = 'x'; stopE2(); } } //MOTOR 3 LEFT SIDE BUTTON void pb5_hit_callback (void) { e3 = false; //m_cmd = 'x'; speed = 1000; wait(1); int stepsToCenter = i3/2; while (stepsToCenter >= 0) { if(stepsToCenter < i3/10) speed = 1500; stepEngine3Left(); stepsToCenter --; } steps3 = 0; m_cmd = 'x'; } //MOTOR 3 RIGHT SIDE BUTTON void pb6_hit_callback (void) { e3 = false; //m_cmd = 'x'; speed = 1000; wait(1); int stepsToCenter = i3/2; while (stepsToCenter >= 0) { if(stepsToCenter < i3/10) speed = 1500; stepEngine3Right(); stepsToCenter --; } //reset steps and stop engine steps3 = 0; m_cmd = 'x'; } void dist(int distance) { //put code here to happen when the distance is changed //stepEngine2Right(); } int main() { mu.startUpdates();//start mesuring the distance 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); //Thread 2 - Ultrasound reading Thread t2(ultraSound); //static char global_direction; printf("Started"); /* //Prosedyre for å midtstille laveste ledd while (true) { stepEngine1Left(); if (e1 == false) { Thread::wait(2000); e1 = true; while(true) { stepEngine1Right(); i1++; 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(); //midstille midterste ledd while (true) { speed = 1000; /* while(bootStep2 >= 0) { bootStep2--; stepEngine2Right(); } */ stepEngine2Left(); if (e2 == false) { e2 = true; while(true) { stepEngine2Right(); i2++; if(e2 == false) break; } Thread::wait(1000); stopAll(); break; }//end if } //checking buttons 5 and 6 pb5.attach_deasserted(&pb5_hit_callback); pb5.setSampleFrequency(); pb6.attach_deasserted(&pb6_hit_callback); pb6.setSampleFrequency(); //Midstille toppledd while (true) { stepEngine3Left(); if (e3 == false) { e3 = true; while(true) { stepEngine3Right(); i3++; if(e3 == false) break; } //pc.printf("HALLO"); //m_cmd = 'z'; Thread::wait(1000); stopAll(); break; }//end if } stopAll(); //normal runtime while (true) { //user button to turn off engines if(SW==0) { while(true) { e3=true; //stopAll(); //Do something else here //mu.checkDistance(); //call checkDistance() as much as possible, as this is where //the class checks if dist needs to be called. //put engines to sleep stepEngine3Right(); if(e3==false) break; } stopAll(); //stepEngine2Right(); // speed=1000; // 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) { steps3 --; stepEngine3Left(); } else if (cmd == '9' && steps3 < 280) { stepEngine3Right(); steps3 ++; } //ENGINE 2 else if (cmd == '4' && steps2 > -242) { steps2 --; stepEngine2Left(); } else if (cmd == '6' && steps2 < 242) { steps2 ++; stepEngine2Right(); } //ENGINE 3 else if (cmd == '1' && steps1 > -242) { steps1--; stepEngine1Left(); } else if (cmd == '3' && steps1 < 242) { steps1++; stepEngine1Right(); } if(cmd == '7') { steps3 --; stepEngine3Left(); } else if (cmd == '9') { stepEngine3Right(); steps3 ++; } //ENGINE 2 else if (cmd == '4') { steps2 --; stepEngine2Left(); } else if (cmd == '6') { steps2 ++; stepEngine2Right(); } //ENGINE 3 else if (cmd == '1') { steps1--; stepEngine1Left(); } else if (cmd == '3') { steps1++; stepEngine1Right(); } if (cmd == 'y') { int step = 100; while (step >= 0) { step--; step2Left3Right(); } m_cmd = 'x'; } if (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