const int SPICLK = 13; const int SPIMOSI = 11; const int SPIMISO = 12; const int SPISS = 10; #define WAIT1 50 #define WAIT2 30 void SPIinit(void) { pinMode (SPICLK, OUTPUT); pinMode (SPIMOSI, OUTPUT); pinMode (SPIMISO, OUTPUT); pinMode (SPISS, OUTPUT); digitalWrite (SPISS, 1); SPCR = _BV(SPE) | _BV(MSTR) | _BV(SPR1) | _BV(SPR0); } unsigned char SPI(char d) { // send character over SPI char received = 0; SPDR = d; while(!(SPSR & _BV(SPIF))); received = SPDR; return (received); } void SPI_startpkt (void) { digitalWrite (SPISS, 0); } void SPI_endpkt (void) { digitalWrite (SPISS, 1); } void set_var (unsigned char addr, unsigned char a, unsigned char v) { SPI_startpkt (); delayMicroseconds (WAIT1); SPI (addr); delayMicroseconds (WAIT2); SPI (a); delayMicroseconds (WAIT2); SPI (v); delayMicroseconds (WAIT2); SPI_endpkt (); } void set_var32 (unsigned char addr, unsigned char a, unsigned long v) { SPI_startpkt (); delayMicroseconds (WAIT1); SPI (addr); delayMicroseconds (WAIT2); SPI (a); delayMicroseconds (WAIT2); SPI (v); delayMicroseconds (WAIT2); SPI (v>>8); delayMicroseconds (WAIT2); SPI (v>>16); delayMicroseconds (WAIT2); SPI (v>>24); delayMicroseconds (WAIT2); SPI_endpkt (); } unsigned long get_var32 (unsigned char addr, unsigned char a) { unsigned long v; SPI_startpkt (); delayMicroseconds (WAIT1); SPI (addr+1); delayMicroseconds (WAIT2); SPI (a); delayMicroseconds (WAIT2); v = (unsigned long)SPI (0x00); delayMicroseconds (WAIT2); v |= (unsigned long)SPI (0x00) << 8; delayMicroseconds (WAIT2); v |= (unsigned long)SPI (0x00) << 16; delayMicroseconds (WAIT2); v |= (unsigned long) SPI (0x00) << 24; delayMicroseconds (WAIT2); SPI_endpkt (); return v; } // The given addresses to talk to. static unsigned char spi_7fet_Raddr = 0x88; static unsigned char spi_7fet_Laddr = 0x70; //Time to pause rotating static unsigned long Pause = 10000; //Amount you want the stepper motor to rotate (0x200 = 25% 0x400 = 50% 0x800= 100% ) RotL means rotation of the left wheel. static unsigned long Rot = 0x200; static unsigned long RotL = 0x200; void setup() { //declare the motor pins as outputs SPIinit (); Serial.begin(9600); char buf[32]; unsigned long RTar; unsigned long RCur; unsigned long LTar; unsigned long LCur; //Makes the current(0x40) + current(0x41) zero of both addresses. Handy if the current + target have a too big difference. set_var32(0x88, 0x40, 0x00); set_var32(0x88, 0x41, 0x00); set_var32(0x70, 0x40, 0x00); set_var32(0x70, 0x41, 0x00); //Change the step delay //50 is 4 times as fast as he normally goes (200) set_var(0x88, 0x43, 200); set_var(0x70, 0x43, 200); //Read current and read target from both the addresses RTar = get_var32 (0x88, 0x41); sprintf (buf, "RIGHT FTarget: %08lx\r\n", RTar ); Serial.write (buf); RCur = get_var32 (0x88, 0x40); sprintf (buf, "RIGHT Fcur: %08lx\r\n", RCur); Serial.write (buf); LTar = get_var32 (0x70, 0x41); sprintf (buf, "LEFT FTarget: %08lx\r\n", LTar ); Serial.write (buf); LCur = get_var32 (0x70, 0x40); sprintf (buf, "LEFT Fcur: %08lx\r\n", LCur); Serial.write (buf); } void loop() { unsigned long RF; unsigned long RCur; unsigned long LCur; unsigned long LF; char buf[32]; //sets rotation values of Rot & RotL set_var32(0x88, 0x42, Rot); set_var32(0x70, 0x42, RotL); //Reads target of both the addresses RF = get_var32 (0x88, 0x41); sprintf (buf, " RTarget: %08lx", RF); Serial.write (buf); LF = get_var32 (0x70, 0x41); sprintf (buf, " LTarget: %08lx\r\n", LF); Serial.write (buf); do { delay(200); //Reads current of both addresses RCur = get_var32 (0x88, 0x40); sprintf (buf, " Rcur: %08lx", RCur); Serial.write (buf); LCur = get_var32 (0x70, 0x40); sprintf (buf, " Lcur: %08lx\r\n", LCur); Serial.write (buf); // looks if current is the same as the target from address 88 RF( Right forward wheel ) } while (RCur != RF); delay(Pause); }