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:
- 24:24c91a6ae6ba
- Parent:
- 23:ad08a8eabc24
- Child:
- 25:321b970eb3ff
--- a/main.cpp Wed May 11 18:44:06 2016 +0000
+++ b/main.cpp Wed May 11 19:39:46 2016 +0000
@@ -23,12 +23,17 @@
DigitalOut IN12(D11);
-static int fart = 1500;
+static int speed = 1500;
static char m_cmd = 'x';
static bool e1 = true;
static bool e2 = true;
static bool e3 = true;
+static int steps1 = 0;
+static int steps2 = 0;
+static int steps3 = 0;
+
+
//Methods to set pins to control direction.
void stepAllRight()
{
@@ -47,7 +52,7 @@
IN10=1;
IN11=0;
IN12=1;
- wait_us(fart); //legg som global variabel "fart"
+ wait_us(speed); //legg som global variabel "fart"
//engine 1
IN1=0;
IN2=1;
@@ -63,7 +68,7 @@
IN10=1;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
//engine 1
IN1=1;
IN2=0;
@@ -79,7 +84,7 @@
IN10=0;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
//engine 1
IN1=1;
IN2=0;
@@ -95,7 +100,7 @@
IN10=0;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepAllLeft()
{
@@ -114,7 +119,7 @@
IN10=0;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
//engine 1
IN1=1;
IN2=0;
@@ -130,7 +135,7 @@
IN10=0;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
//engine 1
IN1=0;
IN2=1;
@@ -146,7 +151,7 @@
IN10=1;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
//engine 1
IN1=0;
IN2=1;
@@ -162,7 +167,7 @@
IN10=1;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepEngine1Left()
@@ -172,22 +177,22 @@
IN2=0;
IN3=0;
IN4=1;
- wait_us(fart);
+ wait_us(speed);
IN1=1;
IN2=0;
IN3=1;
IN4=0;
- wait_us(fart);
+ wait_us(speed);
IN1=0;
IN2=1;
IN3=1;
IN4=0;
- wait_us(fart);
+ wait_us(speed);
IN1=0;
IN2=1;
IN3=0;
IN4=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepEngine1Right()
@@ -197,22 +202,22 @@
IN2=1;
IN3=0;
IN4=1;
- wait_us(fart);
+ wait_us(speed);
IN1=0;
IN2=1;
IN3=1;
IN4=0;
- wait_us(fart);
+ wait_us(speed);
IN1=1;
IN2=0;
IN3=1;
IN4=0;
- wait_us(fart);
+ wait_us(speed);
IN1=1;
IN2=0;
IN3=0;
IN4=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepEngine2Right()
@@ -222,22 +227,22 @@
IN6=1;
IN7=0;
IN8=1;
- wait_us(fart);
+ wait_us(speed);
IN5=0;
IN6=1;
IN7=1;
IN8=0;
- wait_us(fart);
+ wait_us(speed);
IN5=1;
IN6=0;
IN7=1;
IN8=0;
- wait_us(fart);
+ wait_us(speed);
IN5=1;
IN6=0;
IN7=0;
IN8=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepEngine2Left()
@@ -247,22 +252,22 @@
IN6=0;
IN7=0;
IN8=1;
- wait_us(fart);
+ wait_us(speed);
IN5=1;
IN6=0;
IN7=1;
IN8=0;
- wait_us(fart);
+ wait_us(speed);
IN5=0;
IN6=1;
IN7=1;
IN8=0;
- wait_us(fart);
+ wait_us(speed);
IN5=0;
IN6=1;
IN7=0;
IN8=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepEngine3Right()
@@ -272,22 +277,22 @@
IN10=1;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
IN9=0;
IN10=1;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
IN9=1;
IN10=0;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
IN9=1;
IN10=0;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
}
void stepEngine3Left()
@@ -297,22 +302,22 @@
IN10=0;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
IN9=1;
IN10=0;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
IN9=0;
IN10=1;
IN11=1;
IN12=0;
- wait_us(fart);
+ wait_us(speed);
IN9=0;
IN10=1;
IN11=0;
IN12=1;
- wait_us(fart);
+ wait_us(speed);
}
@@ -336,7 +341,7 @@
void wait()
{
- wait_us(fart);
+ wait_us(speed);
}
@@ -408,7 +413,8 @@
//SPECIFY WHICH BUTTON IS PRESSED. NEED ONE CALLBACK FOR EACH BUTTON.
void pb_hit_callback (void)
{
- m_cmd = 'x';
+ e1 = false;
+ //m_cmd = 'x';
/*
@@ -420,6 +426,7 @@
int main()
{
+
pb.mode(PullUp);
pb.attach_deasserted(&pb_hit_callback);
pb.setSampleFrequency();
@@ -459,24 +466,66 @@
*/
-
-
+ // Move motor in one direction
+ while (true)
+ {
+ speed = 2000;
+ stepEngine1Left();
+ if (e1 == false)
+ {
+ Thread::wait(1000);
+ int stepsToCenter = 242;
+
+ while (stepsToCenter >= 0)
+ {
+ stepEngine1Right();
+ stepsToCenter --;
+ }
+ break;
+ }//end if
+
+ }//end while
+ /*
+ while (true)
+ {
+ stepEngine2Right();
+ if (e2 == false)
+ {
+ Thread::wait(1000);
+ int stepsToCenter = 242;
+
+ while (stepsToCenter >= 0)
+ {
+ stepEngine2Left();
+ stepsToCenter --;
+ }
+ break;
+ }//end if
+ }
+ while (true)
+ {
+ stepEngine2Left();
+ if (e3 == false)
+ {
+ Thread::wait(1000);
+ int stepsToCenter = 242;
+
+ while (stepsToCenter >= 0)
+ {
+ stepEngine3Right();
+ stepsToCenter --;
+ }
+ break;
+ }//end if
+ }
+
+ */
+
while (true)
{
-
- if (m_cmd == 't')
- {
- int steps = 50;
- while (steps <= 50)
- {
- step1();
- wait();
- }
-
- }
-
-
-
+ speed = 1500;
+
+
if(m_cmd == 'a')
{
stepAllRight();
@@ -501,19 +550,8 @@
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')
@@ -527,42 +565,37 @@
//Controlling each motor seperately.
//ENGINE 1
- if(m_cmd == '7')
- {
-
-
- stepEngine3Left();
- }
- else if (m_cmd == '9')
+ if(m_cmd == '7' && steps3 > -242)
{
-
+ steps3 --;
+ stepEngine3Left();
+ }
+ else if (m_cmd == '9' && steps3 < 242)
+ {
stepEngine3Right();
+ steps3 ++;
}
//ENGINE 2
- else if (m_cmd == '4')
+ else if (m_cmd == '4' && steps2 > -242)
{
-
-
+ steps2 --;
stepEngine2Left();
}
- else if (m_cmd == '6')
+ else if (m_cmd == '6' && steps2 < 242)
{
-
-
+ steps2 ++;
stepEngine2Right();
}
//ENGINE 3
- else if (m_cmd == '1')
+ else if (m_cmd == '1' && steps1 > -242)
{
-
-
+ steps1--;
stepEngine1Left();
}
- else if (m_cmd == '3')
+ else if (m_cmd == '3' && steps1 < 242)
{
-
-
+ steps1++;
stepEngine1Right();
}
@@ -598,9 +631,10 @@
m_cmd ='x';
}
*/
-
+ //Thread::wait(100);
+ //pc.printf("%d", steps3);
- }
+ } //END WHILE TRUE
} //END Main