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-07-07
- Revision:
- 30:fac5b5624fe3
- Parent:
- 29:00f839aad30e
File content as of revision 30:fac5b5624fe3:
#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);
//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;
//Methods to set pins to control direction.
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);
}
}
//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)
{
stepEngine2Right();
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)
{
stepEngine2Left();
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';
}
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");
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();
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);
break;
}//end if
}
pb5.attach_deasserted(&pb5_hit_callback);
pb5.setSampleFrequency();
pb6.attach_deasserted(&pb6_hit_callback);
pb6.setSampleFrequency();
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);
break;
}//end if
}
//normal runtime
while (true)
{
//user button to turn off engines
if(SW==0)
{
//put engines to sleep
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