Main Code
Dependencies: PinDetect Ulrasonic mbed-rtos mbed WTV020SD_Sound_Breakout_Library
Fork of MC by
main.cpp
- Committer:
- mjhaugsdal
- Date:
- 2016-04-26
- Revision:
- 9:1636083fce43
- Parent:
- 8:5703dadaed07
- Child:
- 10:17a16ceb376b
File content as of revision 9:1636083fce43:
#include "mbed.h" #include "rtos.h" Serial pc(USBTX, USBRX); // tx, rx //Analog Pins //First engine 1-4 DigitalOut IN1(A0); DigitalOut IN2(A1); DigitalOut IN3(A2); DigitalOut IN4(A3); //Second engine 5-8 /* DigitalOut IN5(D3); DigitalOut IN6(D4); DigitalOut IN7(D5); DigitalOut IN8(D6); //Third engine 9-12 DigitalOut IN9(D8); DigitalOut IN10(D9); DigitalOut IN11(D10); DigitalOut IN12(D11); */ static int fart = 1000; static char m_cmd = 'x'; /* void step4Right3() { //engine 2 IN9=0; IN10=1; IN11=0; IN12=1; wait_us(fart); //engine 2 IN9=0; IN10=1; IN11=1; IN12=0; wait_us(fart); //engine 2 IN9=1; IN10=0; IN11=1; IN12=0; wait_us(fart); //engine 2 IN9=1; IN10=0; IN11=0; IN12=1; wait_us(fart); } void step4Right2() { //engine 2 IN5=0; IN6=1; IN7=0; IN8=1; wait_us(fart); //engine 2 IN5=0; IN6=1; IN7=1; IN8=0; wait_us(fart); //engine 2 IN5=1; IN6=0; IN7=1; IN8=0; wait_us(fart); //engine 2 IN5=1; IN6=0; IN7=0; IN8=1; wait_us(fart); } */ void step4Right1() { //engine 1 IN1=0; IN2=1; IN3=0; IN4=1; wait_us(fart); //legg som global variabel "fart" //engine 1 IN1=0; IN2=1; IN3=1; IN4=0; wait_us(fart); //engine 1 IN1=1; IN2=0; IN3=1; IN4=0; wait_us(fart); //engine 1 IN1=1; IN2=0; IN3=0; IN4=1; wait_us(fart); } /* void step4Left3() { //engine 2 IN9=1; IN10=0; IN11=0; IN12=1; wait_us(fart); //engine 2 IN9=1; IN10=0; IN11=1; IN12=0; wait_us(fart); //engine 2 IN9=0; IN10=1; IN11=1; IN12=0; wait_us(fart); //engine 2 IN9=0; IN10=1; IN11=0; IN12=1; wait_us(fart); } void step4Left2() { //engine 2 IN5=1; IN6=0; IN7=0; IN8=1; wait_us(fart); //engine 2 IN5=1; IN6=0; IN7=1; IN8=0; wait_us(fart); //engine 2 IN5=0; IN6=1; IN7=1; IN8=0; wait_us(fart); //engine 2 IN5=0; IN6=1; IN7=0; IN8=1; wait_us(fart); } */ void step4Left1() { //engine 1 IN1=1; IN2=0; IN3=0; IN4=1; wait_us(fart); //engine 1 IN1=1; IN2=0; IN3=1; IN4=0; wait_us(fart); //engine 1 IN1=0; IN2=1; IN3=1; IN4=0; wait_us(fart); //engine 1 IN1=0; IN2=1; IN3=0; IN4=1; wait_us(fart); } void input(void const *args) { while(true) { if(pc.readable()) { m_cmd=pc.getc(); } Thread::wait(10); //pc.printf("%d", steps); } } int main() { //Thread 1 has constant feed from usb Thread t1(input); static char global_direction; printf("Started"); while (true) { //Thread::wait(5); if (m_cmd == 't') { step4Left1(); } //0 STEPS LEFT AT MAX SPEED if (m_cmd == '0') { fart = 1000; //global_direction = '0'; step4Left1(); //step4Left2(); //step4Left3(); //steps = steps + step; //printf("LEFT \n"); //printf("%c \n" + global_direction); } //1 STEPS RIGHT AT MAX SPEED else if (m_cmd == '1') { fart = 1000; //global_direction = '1'; step4Right1(); //step4Right2(); // step4Right3(); } else { //global_direction = 'x'; } if (m_cmd == 'a') { fart = 2000; //global_direction = '1'; step4Right1(); //step4Right2(); //step4Right3(); //steps = steps + step; // printf("%i", &steps); } else if (m_cmd == 'c') { fart = 2000; //global_direction = '0'; step4Left1(); //step4Left2(); //step4Left3(); //steps = steps + step; // printf("%i", &steps); } else if (m_cmd == 'k') { int steps = 50; //Steps = 242 gives a 60' rotation to the left. while (steps >= 0) { step4Left1(); //step4Left2(); //step4Left3(); steps--; } m_cmd = 'x'; } else if (m_cmd == 'l') { int steps = 242; //Steps = 242 gives a 60' rotation to the right. while (steps >= 0) { step4Right1(); //step4Right2(); //step4Right3(); steps--; } m_cmd = 'x'; } } } //END Main