main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 
00004 DigitalIn busy1(p26);
00005 DigitalIn flag1(p25);
00006 DigitalOut cs1(p8);
00007 DigitalOut stck1(p28);
00008 DigitalOut stby1(p27);
00009 
00010 DigitalIn busy2(p22);
00011 DigitalIn flag2(p21);
00012 DigitalOut cs2(p19);
00013 DigitalOut stck2(p24);
00014 DigitalOut stby2(p23);
00015 
00016 DigitalOut led1(LED1);
00017 DigitalOut led2(LED2);
00018 DigitalOut led3(LED3);
00019 DigitalOut led4(LED4);
00020 
00021 SPI spi(p5, p6, p7); // mosi, miso, sclk
00022 Mutex motor1_mutex, motor2_mutex; 
00023 
00024 void writeCommand(int port, unsigned char data)
00025 {
00026     if (port == 1) {
00027         cs1 = 0;
00028         spi.write(data);
00029         cs1 = 1;
00030     } else {
00031         cs2 = 0;
00032         spi.write(data);
00033         cs2 = 1;
00034     }
00035 }
00036 
00037 void writeCommand4(int port, unsigned int data)
00038 {
00039     if (port == 1)
00040         motor1_mutex.lock();
00041     else
00042         motor2_mutex.lock();
00043     writeCommand(port, data >> 24);
00044     writeCommand(port, (data >> 16) & 0xff);
00045     writeCommand(port, (data >> 8) & 0xff);
00046     writeCommand(port, data & 0xff);
00047     if (port == 1)
00048         motor1_mutex.unlock();
00049     else
00050         motor2_mutex.unlock();
00051 }
00052 
00053 unsigned char readCommand(int port)
00054 {
00055     unsigned char result = 0;
00056 
00057     if (port == 1) {
00058         cs1 = 0;
00059         result = spi.write(0);
00060         cs1 = 1;
00061     } else {
00062         cs2 = 0;
00063         result = spi.write(0);
00064         cs2 = 1;
00065     }
00066 
00067     return result;
00068 }
00069 
00070 void initMotor()
00071 {
00072     spi.format(8,0);
00073     busy1.mode(PullUp);
00074     flag1.mode(PullUp);
00075     busy2.mode(PullUp);
00076     flag2.mode(PullUp);
00077     stby1 = 1;
00078     stby2 = 1;
00079     for (int port = 1; port <= 2; port++) {
00080 
00081         writeCommand(port, 0x08);
00082         writeCommand(port, 0x00);
00083 
00084         writeCommand(port, 0x09);
00085         writeCommand(port, 0xff);
00086 
00087         writeCommand(port, 0x0a);
00088         writeCommand(port, 0xff);
00089 
00090         writeCommand(port, 0x0b);
00091         writeCommand(port, 0xff);
00092 
00093         writeCommand(port, 0x0c);
00094         writeCommand(port, 0xff);
00095 
00096         writeCommand(port, 0x13);
00097         writeCommand(port, 0x0f);
00098 
00099         writeCommand(port, 0x14);
00100         writeCommand(port, 0x7f);
00101     }
00102 }
00103 
00104 void MAXspeed_port_1_A()
00105 {
00106     writeCommand(1, 0x07);
00107     writeCommand(1, 0x00);
00108     writeCommand(1, 0x32);
00109 }
00110 
00111 void Step_Startspeed_port_1_A()
00112 {
00113     writeCommand(1, 0x05);
00114     writeCommand(1, 0x00);
00115     writeCommand(1, 0x23);
00116 }
00117 
00118 void Step_Stopspeed_port_1_A()
00119 {
00120     writeCommand(1, 0x06);
00121     writeCommand(1, 0x00);
00122     writeCommand(1, 0x15);
00123 }
00124 
00125 void CW_port_1_A()
00126 {
00127     writeCommand(1, 0x51);
00128     writeCommand(1, 0x00);
00129     writeCommand(1, 0x25);
00130     writeCommand(1, 0x00);
00131 }
00132 
00133 void CCW_port_1_A()
00134 {
00135     writeCommand(1, 0x50);
00136     writeCommand(1, 0x00);
00137     writeCommand(1, 0x10);
00138     writeCommand(1, 0x00);
00139 }
00140 
00141 void CCW_port_1_A2()
00142 {
00143     writeCommand(1, 0x50);
00144     writeCommand(1, 0x00);
00145     writeCommand(1, 0x40);
00146     writeCommand(1, 0x00);
00147 }
00148 //----------------------------------------------
00149 void MAXspeed_port_2_A()
00150 {
00151     writeCommand(2, 0x07);
00152     writeCommand(2, 0x00);
00153     writeCommand(2, 0x32);
00154 }
00155 
00156 void Step_Startspeed_port_2_A()
00157 {
00158     writeCommand(2, 0x05);
00159     writeCommand(2, 0x00);
00160     writeCommand(2, 0x23);
00161 }
00162 
00163 void Step_Stopspeed_port_2_A()
00164 {
00165     writeCommand(2, 0x06);
00166     writeCommand(2, 0x00);
00167     writeCommand(2, 0x15);
00168 }
00169 
00170 void CW_port_2_A()
00171 {
00172     writeCommand(2, 0x51);
00173     writeCommand(2, 0x00);
00174     writeCommand(2, 0x25);
00175     writeCommand(2, 0x00);
00176 }
00177 
00178 void CCW_port_2_A()
00179 {
00180     writeCommand(2, 0x50);
00181     writeCommand(2, 0x00);
00182     writeCommand(2, 0x10);
00183     writeCommand(2, 0x00);
00184 }
00185 
00186 void CCW_port_2_A2()
00187 {
00188     writeCommand(2, 0x50);
00189     writeCommand(2, 0x00);
00190     writeCommand(2, 0x40);
00191     writeCommand(2, 0x00);
00192 }
00193 
00194 //-----------------------------------
00195 void startup1 ()
00196 {
00197     CW_port_1_A();
00198     CW_port_2_A();
00199 }
00200 
00201 void startup2 ()
00202 {
00203     CCW_port_1_A();
00204     CCW_port_2_A();
00205 }
00206 
00207 void power_up_p1_p2 ()
00208 {
00209     writeCommand4(1, 0x51004000);
00210     writeCommand4(2, 0x51004000);
00211     wait_ms (630);
00212     writeCommand(2, 0xb8);
00213     wait_ms (175);
00214     writeCommand(1, 0xb8);
00215 }
00216 
00217 void on ()
00218 {
00219     CW_port_2_A();
00220     wait_ms(200);
00221     writeCommand(2, 0xb8);
00222 }
00223 
00224 void reb1 ()
00225 {
00226     CW_port_2_A();
00227     wait_ms(331);
00228     writeCommand(2, 0xb0);
00229     CCW_port_2_A();
00230     wait_ms(1400);
00231     writeCommand(2, 0xb0);
00232     wait_ms(300);
00233 }
00234 
00235 void reb2 ()
00236 {
00237     writeCommand4(2, 0x51003000);
00238     wait_ms(60);
00239     writeCommand(2, 0xb8);
00240     writeCommand4(2, 0x50003000);
00241 
00242     wait_ms(60);
00243     writeCommand(2, 0xb8);
00244     writeCommand4(2, 0x51003000);
00245     wait_ms(60);
00246     writeCommand(2, 0xb8);
00247     writeCommand4(2, 0x50003000);
00248     wait_ms(60);
00249     writeCommand(2, 0xb8);
00250     writeCommand4(2, 0x51003000);
00251     wait_ms(60);
00252 }
00253 
00254 void reb3 ()
00255 {
00256     writeCommand4(2, 0x51001500);
00257     wait_ms(500);
00258 
00259     writeCommand4(2, 0x50000300);
00260     wait_ms(4000);
00261     writeCommand(2, 0xb0);
00262 }
00263 
00264 void type1_tacho (void const *argument)
00265 {
00266     Thread::wait(2000);
00267 
00268     writeCommand4(2, 0x51001000);
00269     Thread::wait(500);
00270 
00271     writeCommand(2, 0xb0);
00272     Thread::wait(100);
00273 
00274     writeCommand4(2, 0x51002500);
00275     Thread::wait(380);
00276     reb2 ();
00277 
00278     writeCommand4(2, 0x50002000);
00279     Thread::wait(500);
00280     writeCommand(2, 0xb0);
00281 
00282     writeCommand4(2, 0x51000800);
00283     Thread::wait(2050);
00284 
00285     writeCommand4(2, 0x50002000);
00286     Thread::wait(450);
00287     writeCommand(2, 0xb8);
00288     Thread::wait(200);
00289     writeCommand4(2, 0x51000500);//---2---
00290     Thread::wait(1550);
00291 
00292     writeCommand4(2, 0x50002000);
00293     Thread::wait(430);
00294     writeCommand(2, 0xb8);
00295     Thread::wait(200);
00296     writeCommand4(2, 0x51000400);//---3---
00297     Thread::wait(2000);
00298 
00299     writeCommand4(2, 0x50002000);
00300     Thread::wait(400);
00301     writeCommand(2, 0xb8);
00302     Thread::wait(200);
00303     writeCommand4(2, 0x51000200);//---4---
00304     Thread::wait(4000);
00305 
00306     writeCommand4(2, 0x50002000);
00307     Thread::wait(300);
00308     writeCommand(2, 0xb8);
00309     Thread::wait(200);
00310     writeCommand4(2, 0x51000150);//---5---
00311     Thread::wait(3000);
00312     writeCommand4(2, 0x51000100);//---5---
00313     Thread::wait(1500);
00314 
00315     writeCommand4(2, 0x50002000);
00316     Thread::wait(300);
00317     writeCommand(2, 0xb8);
00318     Thread::wait(200);
00319     writeCommand4(2, 0x51000150);//---6---
00320     Thread::wait(3000);
00321     writeCommand4(2, 0x51000100);//---6---
00322     Thread::wait(2500);
00323     
00324     *(bool*)argument = true;
00325 }
00326 
00327 void type1_speed (void const *argument)
00328 {
00329     writeCommand4(1, 0x510004b0);
00330     Thread::wait(1500);
00331     writeCommand4(1, 0x510003e8);
00332     Thread::wait(2000);
00333     writeCommand4(1, 0x51000258);
00334     Thread::wait(2500);
00335     writeCommand4(1, 0x51000190);
00336     Thread::wait(3000);
00337     writeCommand4(1, 0x5100012c);
00338     Thread::wait(3500);
00339     writeCommand4(1, 0x510000fa);
00340     Thread::wait(4500);
00341     writeCommand4(1, 0x51000078);
00342     Thread::wait(5200);
00343     writeCommand(1, 0xb0);
00344     
00345     *(bool*)argument = true;
00346 }
00347 
00348 int main()
00349 {
00350     initMotor();
00351     MAXspeed_port_1_A();
00352     Step_Startspeed_port_1_A();
00353     Step_Stopspeed_port_1_A();
00354 
00355     MAXspeed_port_2_A();
00356     Step_Startspeed_port_2_A();
00357     Step_Stopspeed_port_2_A();
00358 
00359     writeCommand(2, 0x83);
00360     writeCommand(1, 0x83);
00361 
00362     int i = 0;
00363     while(1) {
00364         if (i == 0) {
00365             startup1 ();
00366             wait_ms(200);
00367             startup2 ();
00368             wait_ms(4000);
00369         } else if (i == 1) {
00370             power_up_p1_p2 ();
00371             wait_ms(1000);
00372         } else if (i == 2) {
00373             CCW_port_1_A2();
00374             CCW_port_2_A2();
00375             wait_ms(1000);
00376         } else if (i == 3) {
00377             on ();
00378             wait_ms(2000);
00379         } else if (i == 4) {
00380             reb1 ();
00381         } else if (i == 5) {
00382             bool speed_done = false;
00383             bool tacho_done = false;
00384             Thread speed_thread(&type1_speed, &speed_done);
00385             Thread tacho_thread(&type1_tacho, &tacho_done);
00386             while (!speed_done || !tacho_done)
00387                 wait_ms(10);
00388         } else if (i == 6) {
00389             writeCommand4(1, 0x5000012c);
00390 
00391             writeCommand4(2, 0x50000300);
00392             wait_ms(5000);
00393             reb3 ();
00394             reb3 ();
00395             reb3 ();
00396             reb3 ();
00397             reb3 ();
00398 
00399             writeCommand4(2, 0x50000300);
00400             wait_ms(1800);
00401             writeCommand(1, 0xb8);
00402             writeCommand(2, 0xb8);
00403             wait(10);
00404 
00405         } else if (i == 8) {
00406             CCW_port_1_A();
00407             CCW_port_2_A();
00408             wait_ms(5000);
00409             i = -1;
00410         }
00411         wait_ms(200);
00412 
00413         i++;
00414     }
00415 }
00416 
00417 
00418 
00419 
00420