Robot Bachelor
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 
Robot Bachelor
main.cpp
- Committer:
- mjhaugsdal
- Date:
- 2016-05-11
- Revision:
- 23:ad08a8eabc24
- Parent:
- 22:f6e328f7bd28
- Child:
- 24:24c91a6ae6ba
File content as of revision 23:ad08a8eabc24:
#include "mbed.h"
#include "rtos.h"
#include "PinDetect.h"
Serial pc(USBTX, USBRX); // tx, rx
PinDetect pb(A5);
//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 = 1500;
static char m_cmd = 'x';
static bool e1 = true;
static bool e2 = true;
static bool e3 = true;
//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(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 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(fart);
}
//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);
}
}
//SPECIFY WHICH BUTTON IS PRESSED. NEED ONE CALLBACK FOR EACH BUTTON.
void pb_hit_callback (void)
{
m_cmd = 'x';
/*
*/
}
int main()
{
pb.mode(PullUp);
pb.attach_deasserted(&pb_hit_callback);
pb.setSampleFrequency();
//Thread 1 has constant feed from usb
Thread t1(input);
//static char global_direction;
printf("Started");
/*
BOOT UP AND SYNCHRONIZE
while (true)
{
should move one engine at a time. If not, alot of work has to be done in if/else statements as well as new engine methods..
while(moving bottom part 180 degrees to one side / steps < 0)
{
step bottom engine in one direction.
callback will fire and stop the engine
}
while(moving middle part 180 degrees to one side / steps < 0)
{
step middle engine in one direction
callback fires and stops the engine
}
while(moving middle part 180 degrees to one side / steps < 0)
{
step middle engine in one direction
callback fires and stops the engine
}
//Break the while loop (FALSE)
break;
}
*/
while (true)
{
if (m_cmd == 't')
{
int steps = 50;
while (steps <= 50)
{
step1();
wait();
}
}
if(m_cmd == 'a')
{
stepAllRight();
}
else if (m_cmd == 'b')
{
stepAllLeft();
}
/*
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';
}*/
//Release all motors / Set all pins to 0
if (m_cmd == 'z')
{
stopAll();
}
//Make all engines halt and hold still.
if (m_cmd == 'x')
{
}
//Controlling each motor seperately.
//ENGINE 1
if(m_cmd == '7')
{
stepEngine3Left();
}
else if (m_cmd == '9')
{
stepEngine3Right();
}
//ENGINE 2
else if (m_cmd == '4')
{
stepEngine2Left();
}
else if (m_cmd == '6')
{
stepEngine2Right();
}
//ENGINE 3
else if (m_cmd == '1')
{
stepEngine1Left();
}
else if (m_cmd == '3')
{
stepEngine1Right();
}
else if (m_cmd == 'y')
{
int a = 121;
int b = 60;
int c = 60;
while (a >= 0)
{
stepEngine1Left();
a--;
}
while (b >= 0)
{
stepEngine2Right();
b--;
}
while (c >= 0)
{
stepEngine3Left();
c--;
}
m_cmd = 'x';
}
/*
new_pb = pb;
if (new_pb == '1')
{
m_cmd ='x';
}
*/
}
} //END Main