Revision 1:39b4d6e71f76, committed 2012-11-06

Comitter:

junTMUG

Date:

Tue Nov 06 11:22:47 2012 +0000

Parent:

0:134db3267830

Child:

2:824d7df88ed9

Commit message:

type1_speed?type1_tachp?Thread?

Changed in this revision

--- 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++;
-
     }
-
 }
 
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Nov 06 11:22:47 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#9654a71f5a90