Diff: main.cpp
- Revision:
- 1:39b4d6e71f76
- Parent:
- 0:134db3267830
- Child:
- 2:824d7df88ed9
--- a/main.cpp Tue Nov 06 10:20:54 2012 +0000
+++ b/main.cpp Tue Nov 06 11:22:47 2012 +0000
@@ -1,4 +1,5 @@
#include "mbed.h"
+#include "rtos.h"
DigitalIn busy1(p26);
DigitalIn flag1(p25);
@@ -18,6 +19,7 @@
DigitalOut led4(LED4);
SPI spi(p5, p6, p7); // mosi, miso, sclk
+Mutex motor1_mutex, motor2_mutex;
void writeCommand(int port, unsigned char data)
{
@@ -32,6 +34,22 @@
}
}
+void writeCommand4(int port, unsigned int data)
+{
+ if (port == 1)
+ motor1_mutex.lock();
+ else
+ motor2_mutex.lock();
+ writeCommand(port, data >> 24);
+ writeCommand(port, (data >> 16) & 0xff);
+ writeCommand(port, (data >> 8) & 0xff);
+ writeCommand(port, data & 0xff);
+ if (port == 1)
+ motor1_mutex.unlock();
+ else
+ motor2_mutex.unlock();
+}
+
unsigned char readCommand(int port)
{
unsigned char result = 0;
@@ -82,24 +100,28 @@
writeCommand(port, 0x7f);
}
}
+
void MAXspeed_port_1_A()
{
writeCommand(1, 0x07);
writeCommand(1, 0x00);
writeCommand(1, 0x32);
}
+
void Step_Startspeed_port_1_A()
{
writeCommand(1, 0x05);
writeCommand(1, 0x00);
writeCommand(1, 0x23);
}
+
void Step_Stopspeed_port_1_A()
{
writeCommand(1, 0x06);
writeCommand(1, 0x00);
writeCommand(1, 0x15);
}
+
void CW_port_1_A()
{
writeCommand(1, 0x51);
@@ -107,6 +129,7 @@
writeCommand(1, 0x25);
writeCommand(1, 0x00);
}
+
void CCW_port_1_A()
{
writeCommand(1, 0x50);
@@ -114,6 +137,7 @@
writeCommand(1, 0x10);
writeCommand(1, 0x00);
}
+
void CCW_port_1_A2()
{
writeCommand(1, 0x50);
@@ -128,18 +152,21 @@
writeCommand(2, 0x00);
writeCommand(2, 0x32);
}
+
void Step_Startspeed_port_2_A()
{
writeCommand(2, 0x05);
writeCommand(2, 0x00);
writeCommand(2, 0x23);
}
+
void Step_Stopspeed_port_2_A()
{
writeCommand(2, 0x06);
writeCommand(2, 0x00);
writeCommand(2, 0x15);
}
+
void CW_port_2_A()
{
writeCommand(2, 0x51);
@@ -147,6 +174,7 @@
writeCommand(2, 0x25);
writeCommand(2, 0x00);
}
+
void CCW_port_2_A()
{
writeCommand(2, 0x50);
@@ -154,6 +182,7 @@
writeCommand(2, 0x10);
writeCommand(2, 0x00);
}
+
void CCW_port_2_A2()
{
writeCommand(2, 0x50);
@@ -161,38 +190,37 @@
writeCommand(2, 0x40);
writeCommand(2, 0x00);
}
+
//-----------------------------------
-void stertup1 ()
+void startup1 ()
{
CW_port_1_A();
CW_port_2_A();
}
-void stertup2 ()
+
+void startup2 ()
{
CCW_port_1_A();
CCW_port_2_A();
}
+
void power_up_p1_p2 ()
{
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x40);
- writeCommand(1, 0x00);
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x40);
- writeCommand(2, 0x00);
+ writeCommand4(1, 0x51004000);
+ writeCommand4(2, 0x51004000);
wait_ms (630);
writeCommand(2, 0xb8);
wait_ms (175);
writeCommand(1, 0xb8);
}
+
void on ()
{
CW_port_2_A();
wait_ms(200);
writeCommand(2, 0xb8);
}
+
void reb1 ()
{
CW_port_2_A();
@@ -203,198 +231,113 @@
writeCommand(2, 0xb0);
wait_ms(300);
}
+
void reb2 ()
{
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x30);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x51003000);
wait_ms(60);
writeCommand(2, 0xb8);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x30);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x50003000);
+
wait_ms(60);
writeCommand(2, 0xb8);
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x30);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x51003000);
wait_ms(60);
writeCommand(2, 0xb8);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x30);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x50003000);
wait_ms(60);
writeCommand(2, 0xb8);
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x30);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x51003000);
wait_ms(60);
}
+
void reb3 ()
{
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x15);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x51001500);
wait_ms(500);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x03);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x50000300);
wait_ms(4000);
writeCommand(2, 0xb0);
}
-void type1_tacho ()
-{
- wait_ms(2000);
- writeCommand(2, 0x51); //---1---
- writeCommand(2, 0x00);
- writeCommand(2, 0x10);
- writeCommand(2, 0x00);
- wait_ms(500);
+void type1_tacho (void const *argument)
+{
+ Thread::wait(2000);
+
+ writeCommand4(2, 0x51001000);
+ Thread::wait(500);
writeCommand(2, 0xb0);
- wait_ms(100);
+ Thread::wait(100);
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x25);
- writeCommand(2, 0x00);
- wait_ms(380);
+ writeCommand4(2, 0x51002500);
+ Thread::wait(380);
reb2 ();
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x20);
- writeCommand(2, 0x00);
- wait_ms(500);
+ writeCommand4(2, 0x50002000);
+ Thread::wait(500);
writeCommand(2, 0xb0);
- writeCommand(2, 0x51);
- writeCommand(2, 0x00);
- writeCommand(2, 0x08);
- writeCommand(2, 0x00);
- wait_ms(2050);
+ writeCommand4(2, 0x51000800);
+ Thread::wait(2050);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x20);
- writeCommand(2, 0x00);
- wait_ms(450);
+ writeCommand4(2, 0x50002000);
+ Thread::wait(450);
writeCommand(2, 0xb8);
- wait_ms(200);
- writeCommand(2, 0x51);//---2---
- writeCommand(2, 0x00);
- writeCommand(2, 0x05);
- writeCommand(2, 0x00);
- wait_ms(1550);
+ Thread::wait(200);
+ writeCommand4(2, 0x51000500);//---2---
+ Thread::wait(1550);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x20);
- writeCommand(2, 0x00);
- wait_ms(430);
+ writeCommand4(2, 0x50002000);
+ Thread::wait(430);
writeCommand(2, 0xb8);
- wait_ms(200);
- writeCommand(2, 0x51);//---3---
- writeCommand(2, 0x00);
- writeCommand(2, 0x04);
- writeCommand(2, 0x00);
- wait_ms(2000);
+ Thread::wait(200);
+ writeCommand4(2, 0x51000400);//---3---
+ Thread::wait(2000);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x20);
- writeCommand(2, 0x00);
- wait_ms(400);
+ writeCommand4(2, 0x50002000);
+ Thread::wait(400);
writeCommand(2, 0xb8);
- wait_ms(200);
- writeCommand(2, 0x51);//---4---
- writeCommand(2, 0x00);
- writeCommand(2, 0x02);
- writeCommand(2, 0x00);
- wait_ms(4000);
+ Thread::wait(200);
+ writeCommand4(2, 0x51000200);//---4---
+ Thread::wait(4000);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x20);
- writeCommand(2, 0x00);
- wait_ms(300);
+ writeCommand4(2, 0x50002000);
+ Thread::wait(300);
writeCommand(2, 0xb8);
- wait_ms(200);
- writeCommand(2, 0x51);//---5---
- writeCommand(2, 0x00);
- writeCommand(2, 0x01);
- writeCommand(2, 0x50);
- wait_ms(3000);
- writeCommand(2, 0x51);//---5---
- writeCommand(2, 0x00);
- writeCommand(2, 0x01);
- writeCommand(2, 0x00);
- wait_ms(1500);
+ Thread::wait(200);
+ writeCommand4(2, 0x51000150);//---5---
+ Thread::wait(3000);
+ writeCommand4(2, 0x51000100);//---5---
+ Thread::wait(1500);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x20);
- writeCommand(2, 0x00);
- wait_ms(300);
+ writeCommand4(2, 0x50002000);
+ Thread::wait(300);
writeCommand(2, 0xb8);
- wait_ms(200);
- writeCommand(2, 0x51);//---6---
- writeCommand(2, 0x00);
- writeCommand(2, 0x01);
- writeCommand(2, 0x50);
- wait_ms(3000);
- writeCommand(2, 0x51);//---6---
- writeCommand(2, 0x00);
- writeCommand(2, 0x01);
- writeCommand(2, 0x00);
- wait_ms(2500);
+ Thread::wait(200);
+ writeCommand4(2, 0x51000150);//---6---
+ Thread::wait(3000);
+ writeCommand4(2, 0x51000100);//---6---
+ Thread::wait(2500);
}
-void type1_speed ()
+
+void type1_speed (void const *argument)
{
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x04);
- writeCommand(1, 0xb0);
- wait_ms(1500);
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x03);
- writeCommand(1, 0xe8);
- wait_ms(2000);
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x02);
- writeCommand(1, 0x58);
- wait_ms(2500);
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x01);
- writeCommand(1, 0x90);
- wait_ms(3000);
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x01);
- writeCommand(1, 0x2c);
- wait_ms(3500);
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x00);
- writeCommand(1, 0xfa);
- wait_ms(4500);
- writeCommand(1, 0x51);
- writeCommand(1, 0x00);
- writeCommand(1, 0x00);
- writeCommand(1, 0x78);
- wait_ms(5200);
+ writeCommand4(1, 0x510004b0);
+ Thread::wait(1500);
+ writeCommand4(1, 0x510003e8);
+ Thread::wait(2000);
+ writeCommand4(1, 0x51000258);
+ Thread::wait(2500);
+ writeCommand4(1, 0x51000190);
+ Thread::wait(3000);
+ writeCommand4(1, 0x5100012c);
+ Thread::wait(3500);
+ writeCommand4(1, 0x510000fa);
+ Thread::wait(4500);
+ writeCommand4(1, 0x51000078);
+ Thread::wait(5200);
writeCommand(1, 0xb0);
}
@@ -415,9 +358,9 @@
int i = 0;
while(1) {
if (i == 0) {
- stertup1 ();
+ startup1 ();
wait_ms(200);
- stertup2 ();
+ startup2 ();
wait_ms(4000);
} else if (i == 1) {
power_up_p1_p2 ();
@@ -432,21 +375,12 @@
} else if (i == 4) {
reb1 ();
} else if (i == 5) {
-
- //type1_demo ();
- type1_speed ();
- type1_tacho ();
-
+ Thread speed_thread(&type1_speed);
+ Thread tacho_thread(&type1_tacho);
} else if (i == 6) {
- writeCommand(1, 0x50);
- writeCommand(1, 0x00);
- writeCommand(1, 0x01);
- writeCommand(1, 0x2c);
+ writeCommand4(1, 0x5000012c);
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x03);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x50000300);
wait_ms(5000);
reb3 ();
reb3 ();
@@ -454,11 +388,7 @@
reb3 ();
reb3 ();
-
- writeCommand(2, 0x50);
- writeCommand(2, 0x00);
- writeCommand(2, 0x03);
- writeCommand(2, 0x00);
+ writeCommand4(2, 0x50000300);
wait_ms(1800);
writeCommand(1, 0xb8);
writeCommand(2, 0xb8);
@@ -473,9 +403,7 @@
wait_ms(200);
i++;
-
}
-
}
masayuki shindo
