const char EOPmarker = '.'; //This is the end of packet marker
char serialbuf[32]; //This gives the incoming serial some room. Change it if you want a longer incoming.
#include <string.h> // we'll need this for subString
#define MAX_STRING_LEN 20 // like 3 lines above, change as needed.
#include <LiquidCrystal.h> //we'll need this for the lcd
LiquidCrystal lcd(7, 8, 9, 10, 11, 12); //pins for the lcd, I set it up using the ladyada tutorial.
#include <Servo.h>
Servo left_servo;
Servo right_servo;
Servo esc;
int esc_pwm = 6;
int left_servo_pos;
int right_servo_pos;
int current;
int prophot = 2;
int SER_Pin = 8; //pin 14 on the 75HC595
int RCLK_Pin = 9; //pin 12 on the 75HC595
int SRCLK_Pin = 10; //pin 11 on the 75HC595
#define number_of_74hc595s 1 //How many of the shift registers - change this
#define numOfRegisterPins number_of_74hc595s * 8 //do not touch
boolean registers[numOfRegisterPins];
void setup(){
lcd.begin(16, 2);
left_servo.attach(3);
right_servo.attach(5);
esc.attach(6);
pinMode(prophot, INPUT);
pinMode(esc_pwm, OUTPUT);
current = 0;
pinMode(SER_Pin, OUTPUT);
pinMode(RCLK_Pin, OUTPUT);
pinMode(SRCLK_Pin, OUTPUT);
for(int i = 0; i < 180; i += 1){
esc.write(i);
Serial.println(i);
delay(15);
}
esc.write(0);
clearRegisters();
writeRegisters();
Serial.begin(9600); //changing this to other speeds has not been tested using this meathod
}
void loop() {
setRegisterPin(1, HIGH);
writeRegisters();
if (Serial.available() > 0) { //makes sure something is ready to be read
lcd.clear(); //clears for incoming stuff, won't clear if there isin't data to be read
static int bufpos = 0; //starts the buffer back at the first position in the incoming serial.read
char inchar = Serial.read(); //assigns one byte (as serial.read()'s only input one byte at a time
if (inchar != EOPmarker) { //if the incoming character is not the byte that is the incoming package ender
serialbuf[bufpos] = inchar; //the buffer position in the array get assigned to the current read
bufpos++; //once that has happend the buffer advances, doing this over and over again until the end of package marker is read.
}
else { //once the end of package marker has been read
serialbuf[bufpos] = 0; //restart the buff
bufpos = 0; //restart the position of the buff
left_servo_pos = atoi(subStr(serialbuf, ",", 1));
setRegisterPin(1, HIGH);
writeRegisters();
left_servo.write(left_servo_pos);
setRegisterPin(1, LOW);
writeRegisters();
lcd.setCursor(0, 1);
right_servo_pos = atoi(subStr(serialbuf, ",", 2));
setRegisterPin(3, HIGH);
writeRegisters();
right_servo.write(right_servo_pos);
setRegisterPin(3, LOW);
writeRegisters();
if (digitalRead(prophot) == HIGH){
esc.write(atoi(subStr(serialbuf, ",", 3)));
}
if (digitalRead(prophot) == LOW){
esc.write(0);
}
}
}
}
// below is just function logic, which I do not fully understand. but it works.
char* subStr (char* input_string, char *separator, int segment_number) {
char *act, *sub, *ptr;
static char copy[MAX_STRING_LEN];
int i;
strcpy(copy, input_string);
for (i = 1, act = copy; i <= segment_number; i++, act = NULL) {
sub = strtok_r(act, separator, &ptr);
if (sub == NULL) break;
}
return sub;
}
void clearRegisters(){
for(int i = numOfRegisterPins - 1; i >= 0; i--){
registers[i] = LOW;
}
}
void writeRegisters(){
digitalWrite(RCLK_Pin, LOW);
for(int i = numOfRegisterPins - 1; i >= 0; i--){
digitalWrite(SRCLK_Pin, LOW);
int val = registers[i];
digitalWrite(SER_Pin, val);
digitalWrite(SRCLK_Pin, HIGH);
}
digitalWrite(RCLK_Pin, HIGH);
}
//set an individual pin HIGH or LOW
void setRegisterPin(int index, int value){
registers[index] = value;
}
void spinto(int target) {
setRegisterPin(3, HIGH);
writeRegisters();
while(current > target){
//Serial.println(current);
esc.write(current);
delay(15);
current--;
}
while(current < target){
//Serial.println(current);
esc.write(current);
delay(15);
current++;
}
setRegisterPin(3, LOW);
writeRegisters();
}
//www.esologic.com
//Thanks to http://arduino.cc/forum/index.php?topic=119429
