Rev Author Line No. Line
2934 jacho 1 #include <SPI.h>
2  
3 #include "WProgram.h"
4 void command(char* buffer, unsigned int len);
5 void goTo(unsigned int pos);
6 void setParam(unsigned int reg, unsigned int val, unsigned int len);
7 void setup();
8 int readInt();
9 void loop();
10 void command(char* buffer, unsigned int len)
11 {
12 for (int i = 0; i < len; i++) {
13 delay(10);
14 digitalWrite(SS, 0);
15 SPI.transfer(buffer[i]);
16 digitalWrite(SS, 1);
17 }
18 }
19  
20 void goTo(unsigned int pos)
21 {
22 char buffer[4];
23 buffer[0] = 0x60;
24 buffer[1] = pos >> 16;
25 buffer[2] = (pos >> 8) & 0xff;
26 buffer[3] = pos & 0xff;
27 command(buffer, 4);
28 }
29  
30 void setParam(unsigned int reg, unsigned int val, unsigned int len)
31 {
32 char buffer[4];
33 buffer[0] = reg;
34  
35 /* BUG: reg? */
36 for (int i = 0; i < len; i++)
37 buffer[i + 1] = (reg >> (8 * ((len - 1) - i))) & 0xff;
38  
39 command(buffer, 1 + len);
40 }
41  
42 void setup()
43 {
44 Serial.begin(9600);
45 SPI.begin(SPI_MASTER, MSBFIRST, SPI_MODE3, SPI_CLOCK_DIV32);
46  
47 setParam(5, 0xd0, 2); // ACC
48 setParam(6, 0xd0, 2); // DEC
49 setParam(7, 0, 2); // MAX_SPEED
50 Serial.println("Testovaci program pro HBSTEP01A");
51 }
52  
53 int readInt()
54 {
55 char line[100];
56 int i = 0;
57  
58 while (true) {
59 if (Serial.available()) {
60 byte c = Serial.read();
61  
62 if (c == '\n' || c == '\r') {
63 Serial.println();
64 line[i] = '\0';
65 return atoi(line);
66 }
67  
68 Serial.write(c);
69 line[i++] = c;
70 }
71 }
72 }
73  
74 void loop()
75 {
76 int speed = readInt();
77  
78 int dir = speed > 0;
79 if (!dir)
80 speed = -speed;
81  
82 char buffer[4];
83 buffer[0] = 0x50 | dir;
84 buffer[1] = (speed >> 16) & 0xff;
85 buffer[2] = (speed >> 8) & 0xff;
86 buffer[3] = speed & 0xff;
87  
88 command(buffer, 4);
89 }
90