USN Robotics
/
Robot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: PinDetect Ulrasonic mbed-rtos mbed WTV020SD_Sound_Breakout_Library
Fork of
MC
by 
Robot Bachelor
main.cpp
- Committer:
- mjhaugsdal
- Date:
- 2016-05-03
- Revision:
- 20:d90bd1718078
- Parent:
- 19:79600d3509d7
- Child:
- 21:9f3ae352ee63
File content as of revision 20:d90bd1718078:
#include "mbed.h"
#include "rtos.h"
Serial pc(USBTX, USBRX); // tx, rx
//Analog Pins
//First engine
DigitalOut IN1(A0);
DigitalOut IN2(A1);
DigitalOut IN3(A2);
DigitalOut IN4(A3);
//Second engine
DigitalOut IN5(D3);
DigitalOut IN6(D4);
DigitalOut IN7(D5);
DigitalOut IN8(D6);
//Third Engine
DigitalOut IN9(D8);
DigitalOut IN10(D9);
DigitalOut IN11(D10);
DigitalOut IN12(D11);
static int fart = 1000;
static char m_cmd = 'x';
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(fart); //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(fart);
//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(fart);
//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(fart);
}
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(fart);
//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(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(fart);
//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(fart);
}
void stepEngine1Left()
{
//engine 1
IN1=1;
IN2=0;
IN3=0;
IN4=1;
wait_us(fart);
IN1=1;
IN2=0;
IN3=1;
IN4=0;
wait_us(fart);
IN1=0;
IN2=1;
IN3=1;
IN4=0;
wait_us(fart);
IN1=0;
IN2=1;
IN3=0;
IN4=1;
wait_us(fart);
}
void stepEngine1Right()
{
//engine 1
IN1=0;
IN2=1;
IN3=0;
IN4=1;
wait_us(fart);
IN1=0;
IN2=1;
IN3=1;
IN4=0;
wait_us(fart);
IN1=1;
IN2=0;
IN3=1;
IN4=0;
wait_us(fart);
IN1=1;
IN2=0;
IN3=0;
IN4=1;
wait_us(fart);
}
void stepEngine2Right()
{
//engine 1
IN5=0;
IN6=1;
IN7=0;
IN8=1;
wait_us(fart);
IN5=0;
IN6=1;
IN7=1;
IN8=0;
wait_us(fart);
IN5=1;
IN6=0;
IN7=1;
IN8=0;
wait_us(fart);
IN5=1;
IN6=0;
IN7=0;
IN8=1;
wait_us(fart);
}
void stepEngine2Left()
{
//engine 1
IN5=1;
IN6=0;
IN7=0;
IN8=1;
wait_us(fart);
IN5=1;
IN6=0;
IN7=1;
IN8=0;
wait_us(fart);
IN5=0;
IN6=1;
IN7=1;
IN8=0;
wait_us(fart);
IN5=0;
IN6=1;
IN7=0;
IN8=1;
wait_us(fart);
}
void stepEngine3Right()
{
//engine 1
IN9=0;
IN10=1;
IN11=0;
IN12=1;
wait_us(fart);
IN9=0;
IN10=1;
IN11=1;
IN12=0;
wait_us(fart);
IN9=1;
IN10=0;
IN11=1;
IN12=0;
wait_us(fart);
IN9=1;
IN10=0;
IN11=0;
IN12=1;
wait_us(fart);
}
void stepEngine3Left()
{
//engine 1
IN9=1;
IN10=0;
IN11=0;
IN12=1;
wait_us(fart);
IN9=1;
IN10=0;
IN11=1;
IN12=0;
wait_us(fart);
IN9=0;
IN10=1;
IN11=1;
IN12=0;
wait_us(fart);
IN9=0;
IN10=1;
IN11=0;
IN12=1;
wait_us(fart);
}
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;
}
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(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);
/*
else if (m_cmd == 'k')
{
int steps = 242;
//Steps = 242 gives a 60' rotation to the left.
while (steps >= 0)
{
stepAllLeft();
steps--;
}
m_cmd = 'x';
}
else if (m_cmd == 'l')
{
int steps = 242;
//Steps = 242 gives a 60' rotation to the right.
while (steps >= 0)
{
stepAllRight();
steps--;
}
m_cmd = 'x';
}*/
if (m_cmd == 'z')
{
stopAll();
}
if (m_cmd == 'x')
{
Thread::wait(5);
}
//ENGINE 1
if(m_cmd == '7')
{
//fart = 1200;
stopE1();
stepEngine1Left();
}
else if (m_cmd == '9')
{
//fart = 1200;
stopE1();
stepEngine1Right();
}
//ENGINE 2
else if (m_cmd == '4')
{
//fart = 1200;
stopE2();
stepEngine2Left();
}
else if (m_cmd == '6')
{
//fart = 1200;
stopE2();
stepEngine2Right();
}
//ENGINE 3
else if (m_cmd == '1')
{
//fart = 1200;
stopE3();
stepEngine3Left();
}
else if (m_cmd == '3')
{
//fart = 1200;
stopE3();
stepEngine3Right();
}
}
} //END Main