masayuki shindo
/
L6470Stepper_test4
consept_meter
main.cpp
- Committer:
- junTMUG
- Date:
- 2012-11-06
- Revision:
- 3:accb9e3920f9
- Parent:
- 2:824d7df88ed9
File content as of revision 3:accb9e3920f9:
#include "mbed.h" #include "rtos.h" DigitalIn busy1(p26); DigitalIn flag1(p25); DigitalOut cs1(p8); DigitalOut stck1(p28); DigitalOut stby1(p27); DigitalIn busy2(p22); DigitalIn flag2(p21); DigitalOut cs2(p19); DigitalOut stck2(p24); DigitalOut stby2(p23); DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); SPI spi(p5, p6, p7); // mosi, miso, sclk Mutex motor1_mutex, motor2_mutex; void writeCommand(int port, unsigned char data) { if (port == 1) { cs1 = 0; spi.write(data); cs1 = 1; } else { cs2 = 0; spi.write(data); cs2 = 1; } } 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; if (port == 1) { cs1 = 0; result = spi.write(0); cs1 = 1; } else { cs2 = 0; result = spi.write(0); cs2 = 1; } return result; } void initMotor() { spi.format(8,0); busy1.mode(PullUp); flag1.mode(PullUp); busy2.mode(PullUp); flag2.mode(PullUp); stby1 = 1; stby2 = 1; for (int port = 1; port <= 2; port++) { writeCommand(port, 0x08); writeCommand(port, 0x00); writeCommand(port, 0x09); writeCommand(port, 0xff); writeCommand(port, 0x0a); writeCommand(port, 0xff); writeCommand(port, 0x0b); writeCommand(port, 0xff); writeCommand(port, 0x0c); writeCommand(port, 0xff); writeCommand(port, 0x13); writeCommand(port, 0x0f); writeCommand(port, 0x14); 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); writeCommand(1, 0x00); writeCommand(1, 0x25); writeCommand(1, 0x00); } void CCW_port_1_A() { writeCommand(1, 0x50); writeCommand(1, 0x00); writeCommand(1, 0x10); writeCommand(1, 0x00); } void CCW_port_1_A2() { writeCommand(1, 0x50); writeCommand(1, 0x00); writeCommand(1, 0x40); writeCommand(1, 0x00); } //---------------------------------------------- void MAXspeed_port_2_A() { writeCommand(2, 0x07); 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); writeCommand(2, 0x00); writeCommand(2, 0x25); writeCommand(2, 0x00); } void CCW_port_2_A() { writeCommand(2, 0x50); writeCommand(2, 0x00); writeCommand(2, 0x10); writeCommand(2, 0x00); } void CCW_port_2_A2() { writeCommand(2, 0x50); writeCommand(2, 0x00); writeCommand(2, 0x40); writeCommand(2, 0x00); } //----------------------------------- void startup1 () { CW_port_1_A(); CW_port_2_A(); } void startup2 () { CCW_port_1_A(); CCW_port_2_A(); } void power_up_p1_p2 () { 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(); wait_ms(331); writeCommand(2, 0xb0); CCW_port_2_A(); wait_ms(1400); writeCommand(2, 0xb0); wait_ms(300); } void reb2 () { writeCommand4(2, 0x51003000); wait_ms(60); writeCommand(2, 0xb8); writeCommand4(2, 0x50003000); wait_ms(60); writeCommand(2, 0xb8); writeCommand4(2, 0x51003000); wait_ms(60); writeCommand(2, 0xb8); writeCommand4(2, 0x50003000); wait_ms(60); writeCommand(2, 0xb8); writeCommand4(2, 0x51003000); wait_ms(60); } void reb3 () { writeCommand4(2, 0x51001500); wait_ms(500); writeCommand4(2, 0x50000300); wait_ms(4000); writeCommand(2, 0xb0); } void type1_tacho (void const *argument) { Thread::wait(2000); writeCommand4(2, 0x51001000); Thread::wait(500); writeCommand(2, 0xb0); Thread::wait(100); writeCommand4(2, 0x51002500); Thread::wait(380); reb2 (); writeCommand4(2, 0x50002000); Thread::wait(500); writeCommand(2, 0xb0); writeCommand4(2, 0x51000800); Thread::wait(2050); writeCommand4(2, 0x50002000); Thread::wait(450); writeCommand(2, 0xb8); Thread::wait(200); writeCommand4(2, 0x51000500);//---2--- Thread::wait(1550); writeCommand4(2, 0x50002000); Thread::wait(430); writeCommand(2, 0xb8); Thread::wait(200); writeCommand4(2, 0x51000400);//---3--- Thread::wait(2000); writeCommand4(2, 0x50002000); Thread::wait(400); writeCommand(2, 0xb8); Thread::wait(200); writeCommand4(2, 0x51000200);//---4--- Thread::wait(4000); writeCommand4(2, 0x50002000); Thread::wait(300); writeCommand(2, 0xb8); Thread::wait(200); writeCommand4(2, 0x51000150);//---5--- Thread::wait(3000); writeCommand4(2, 0x51000100);//---5--- Thread::wait(1500); writeCommand4(2, 0x50002000); Thread::wait(300); writeCommand(2, 0xb8); Thread::wait(200); writeCommand4(2, 0x51000150);//---6--- Thread::wait(3000); writeCommand4(2, 0x51000100);//---6--- Thread::wait(2500); *(bool*)argument = true; } void type1_speed (void const *argument) { 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); *(bool*)argument = true; } int main() { initMotor(); MAXspeed_port_1_A(); Step_Startspeed_port_1_A(); Step_Stopspeed_port_1_A(); MAXspeed_port_2_A(); Step_Startspeed_port_2_A(); Step_Stopspeed_port_2_A(); writeCommand(2, 0x83); writeCommand(1, 0x83); int i = 0; while(1) { if (i == 0) { startup1 (); wait_ms(200); startup2 (); wait_ms(4000); } else if (i == 1) { power_up_p1_p2 (); wait_ms(1000); } else if (i == 2) { CCW_port_1_A2(); CCW_port_2_A2(); wait_ms(1000); } else if (i == 3) { on (); wait_ms(2000); } else if (i == 4) { reb1 (); } else if (i == 5) { bool speed_done = false; bool tacho_done = false; Thread speed_thread(&type1_speed, &speed_done); Thread tacho_thread(&type1_tacho, &tacho_done); while (!speed_done || !tacho_done) wait_ms(10); } else if (i == 6) { writeCommand4(1, 0x5000012c); writeCommand4(2, 0x50000300); wait_ms(5000); reb3 (); reb3 (); reb3 (); reb3 (); reb3 (); writeCommand4(2, 0x50000300); wait_ms(1800); writeCommand(1, 0xb8); writeCommand(2, 0xb8); wait(10); } else if (i == 8) { CCW_port_1_A(); CCW_port_2_A(); wait_ms(5000); i = -1; } wait_ms(200); i++; } }