I’m new with programming, Arduino and Pololu Micro serial controller. Can anyone help me get this code in order to center 1 servo? I’ll be using 4 servos and if I can get help making one operating I should be able to get the others working.
Currently this is the code I’ve put together from examples in other projects and I know there is more than one mistake. I am using Pololu Mode with the jumper removed.
#include <ctype.h>
#define bit9600Delay 84
#define halfBit9600Delay 42
#define bit4800Delay 188
#define halfBit4800Delay 94
byte rx = 6;
byte tx = 7;
byte SWval;
void setup() {
pinMode(rx,INPUT);
pinMode(tx,OUTPUT);
digitalWrite(tx,HIGH);
}
void put(int servo, int angle){
unsigned char buff[6];
DWORD len;
unsigned short int temp;
unsigned char pos_hi,pos_low;
temp=angle&0x1f80;
pos_hi=temp>>7;
pos_low=angle & 0x7f;
buff[0]=0x80;//start byte
buff[1]=0x01;//device id
buff[2]=0x04;//command number
buff[3]=servo;//servo number
buff[4]=pos_hi;//data1
buff[5]=pos_low;//data2
WriteFile(comPort, &buff, 6, &len, 0);
Serial.print("Servo %d Set to %d\n", servo, angle);
}
void SWprint(int data)
{
byte mask;
//startbit
digitalWrite(tx,LOW);
delayMicroseconds(bit9600Delay);
for (mask = 0x01; mask>0; mask <<= 1) {
if (data & mask){ // choose bit
digitalWrite(tx,HIGH); // send 1
}
else{
digitalWrite(tx,LOW); // send 0
}
delayMicroseconds(bit9600Delay);
}
//stop bit
digitalWrite(tx, HIGH);
delayMicroseconds(bit9600Delay);
}
int SWread()
{
byte val = 0;
while (digitalRead(rx));
//wait for start bit
if (digitalRead(rx) == LOW) {
delayMicroseconds(halfBit9600Delay);
for (int offset = 0; offset < 8; offset++) {
delayMicroseconds(bit9600Delay);
val |= digitalRead(rx) << offset;
}
//wait for stop bit + extra
delayMicroseconds(bit9600Delay);
delayMicroseconds(bit9600Delay);
return val;
}
}
void loop()
{
SWval = SWread();
SWprint(toupper(SWval));
}
Thanks,
Phil