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
Diff: main.cpp
- Revision:
- 17:ff0021c4dcdc
- Parent:
- 16:a3555918c025
- Child:
- 18:ef02a9014491
--- a/main.cpp Tue Apr 26 14:13:34 2016 +0000
+++ b/main.cpp Tue Apr 26 14:25:29 2016 +0000
@@ -26,7 +26,7 @@
-void step4Right()
+void stepAllRight()
{
//engine 1
IN1=0;
@@ -38,6 +38,11 @@
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;
@@ -49,6 +54,11 @@
IN6=1;
IN7=1;
IN8=0;
+ //engine 3
+ IN9=0;
+ IN10=1;
+ IN11=1;
+ IN12=0;
wait_us(fart);
//engine 1
IN1=1;
@@ -60,6 +70,11 @@
IN6=0;
IN7=1;
IN8=0;
+ //engine 3
+ IN9=1;
+ IN10=0;
+ IN11=1;
+ IN12=0;
wait_us(fart);
//engine 1
IN1=1;
@@ -71,9 +86,14 @@
IN6=0;
IN7=0;
IN8=1;
+ //engine 3
+ IN9=1;
+ IN10=0;
+ IN11=0;
+ IN12=1;
wait_us(fart);
}
-void step4Left()
+void stepAllLeft()
{
//engine 1
IN1=1;
@@ -85,6 +105,11 @@
IN6=0;
IN7=0;
IN8=1;
+ //engine 3
+ IN9=1;
+ IN10=0;
+ IN11=0;
+ IN12=1;
wait_us(fart);
//engine 1
IN1=1;
@@ -96,6 +121,11 @@
IN6=0;
IN7=1;
IN8=0;
+ //engine 3
+ IN9=1;
+ IN10=0;
+ IN11=1;
+ IN12=0;
wait_us(fart);
//engine 1
IN1=0;
@@ -107,6 +137,11 @@
IN6=1;
IN7=1;
IN8=0;
+ //engine 3
+ IN9=0;
+ IN10=1;
+ IN11=1;
+ IN12=0;
wait_us(fart);
//engine 1
IN1=0;
@@ -118,10 +153,116 @@
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 input(void const *args)
{
while(true)
@@ -130,8 +271,8 @@
if(pc.readable())
{ m_cmd=pc.getc();
- }
-
+ }
+
Thread::wait(10);
//pc.printf("%d", steps);
}
@@ -149,12 +290,25 @@
{
//Thread::wait(5);
+
+ if(m_cmd == 't')
+ {
+ stepAllLeft();
+
+ }
+ if(m_cmd == 'y')
+ {
+ stepAllRight();
+
+ }
+
+
//0 STEPS LEFT AT MAX SPEED
if (m_cmd == '0')
{
fart = 1000;
//global_direction = '0';
- step4Left();
+ stepAllLeft();
//steps = steps + step;
@@ -166,7 +320,7 @@
{
fart = 1000;
//global_direction = '1';
- step4Right();
+ stepAllRight();
}
else
{
@@ -177,7 +331,7 @@
{
fart = 2000;
//global_direction = '1';
- step4Right();
+ stepAllRight();
//steps = steps + step;
// printf("%i", &steps);
@@ -187,7 +341,7 @@
{
fart = 2000;
//global_direction = '0';
- step4Left();
+ stepAllLeft();
//steps = steps + step;
// printf("%i", &steps);
@@ -200,7 +354,7 @@
//Steps = 242 gives a 60' rotation to the left.
while (steps >= 0)
{
- step4Left();
+ stepAllLeft();
steps--;
}
m_cmd = 'x';
@@ -211,7 +365,7 @@
//Steps = 242 gives a 60' rotation to the right.
while (steps >= 0)
{
- step4Right();
+ stepAllRight();
steps--;
}
m_cmd = 'x';