I was hoping to avoid that since it is so extensive, but here it is.
/*
TODO:
Find why it crashes
DONE:
*Change all counts to unsigned longs
*Add parsing of stepper motor packets
*Add pulsing of a digital pin
*Add selection of direction via a digital pin
*Add control to step period via packets
*Locate scizzor lift by reed switches
Return command list:
00 - No error
01 - Data Received
02 - Sync char not found
03 - Incomplete packet
04 - Packet Loaded
05 - Stepper udpated
06 - Dynamixel updated
07 - Drive camera toggled
Command List:
00 - Check status
01 - Update Scizzor lift
02 - Update dynamixels
03 - Toggle Drive Camera
04 - Toggle Light source
*/
//#include <avr/io.h>
//#include <avr/wdt.h>
#include <SPI.h>
#include <Ethernet.h>
#include "TimerOne.h"
#include "TimerThree.h"
#define BUFFER_SIZE 64
#define DYNA_TOGGLE_PIN 53
#define SCIZZOR_STEP_PIN 38
#define SCIZZOR_DIR_PIN 39
#define REED_TOP_PIN 22
#define REED_BOTTOM_PIN 23
#define LIGHT_CONTROL_PIN 13
#define DRIVE_CAMERA_PIN 25
#define DYNA_DELAY 1 // Delay in MS
//********************* Ethernet stuff **************
//byte mac[] = {0x90, 0xa2, 0xda, 0x0d, 0x7d, 0xb8}; // Board 1
byte mac[] = {0x90, 0xa2, 0xda, 0x0d, 0x7f, 0x5e}; // Board 2
IPAddress ip(192,168,1, 177);
IPAddress gateway(192,168,1, 1);
IPAddress subnet(255, 255, 0, 0);
EthernetServer server = EthernetServer(23);// telnet defaults to port 23
EthernetClient client;
// ***************** Packet stuff *****************
char packetBuffer[BUFFER_SIZE] = {' '};
int i = 0;
unsigned int temp;
unsigned int checksum;
bool packetLoaded = false;
char bufReturn[BUFFER_SIZE] = {' '};
int itReturn = 0;
boolean alreadyConnected = false;
char payLoad[BUFFER_SIZE] = {' '};
int packetStart = 0;
int packetType = -1;
bool clientFirstConnected = false;
char dynamixelReturnData[BUFFER_SIZE] = {0x00};
int itDyna = 0;
//*********************** Stepper motor stuff *********************
unsigned long ScizzorSteps = 500000; // 0 Is the bottom of the lift
bool scizzorOn = false;
unsigned long scizzorBotUnlatch = 27391; // 29532
unsigned long scizzorBotLatch = 21421; // 23372
unsigned long maxScizzorSteps = 440687;
char x = ' ';
char z = ' ';
char p = ' ';
char o = ' ';
long int period = 100;
int length = 0;
int packetSize = 0;
bool dataRead = false;
bool dynamixelLED = false;
void setup()
{
Ethernet.begin(mac, ip, gateway, subnet);// initialize the ethernet device
server.begin();// start listening for clients
Serial.begin(57600);
Serial3.begin(57600);
pinMode(DYNA_TOGGLE_PIN, OUTPUT); // When pin is high, can write data, when pin is low, read data,
digitalWrite(DYNA_TOGGLE_PIN, HIGH);
char initialPacket[16] = { 0x02, 0x02, 0x06, 0x06,
0x01, 0x05, 0x03, 0x1E, 0x07, 0x08,
0x00, 0x05, 0x03, 0x1E, 0xFD, 0x05};
for(int i = 0; i < 16; i++)
{
payLoad[i] = initialPacket[i];
}
updateDynamixels();
pinMode(SCIZZOR_STEP_PIN, OUTPUT); // Scizzor Step
pinMode(SCIZZOR_DIR_PIN, OUTPUT); // Scizzor Dir when LOW raises lift, when HIGH lowers lift
pinMode(REED_TOP_PIN, INPUT); // Reed top
pinMode(REED_BOTTOM_PIN, INPUT); // Reed bottom
pinMode(DRIVE_CAMERA_PIN, OUTPUT);
pinMode(LIGHT_CONTROL_PIN, OUTPUT);
pinMode(10,OUTPUT);
digitalWrite(10,HIGH);
// ***************** Set scizzor lift absolute position based on reed switches ***********************
if(digitalRead(REED_BOTTOM_PIN)) // Scizzor lift is at bottom, need to move it up to determine unlatching point
{
digitalWrite(SCIZZOR_DIR_PIN,LOW);
while(digitalRead(REED_BOTTOM_PIN))
{
digitalWrite(SCIZZOR_STEP_PIN,!digitalRead(SCIZZOR_STEP_PIN));
delayMicroseconds(50);
}
ScizzorSteps = scizzorBotUnlatch; // Sets the location to the known unlatching point
}
else
{
digitalWrite(SCIZZOR_DIR_PIN,HIGH);
while(!digitalRead(REED_BOTTOM_PIN))
{
digitalWrite(SCIZZOR_STEP_PIN,!digitalRead(SCIZZOR_STEP_PIN));// Lower until switch latches
delayMicroseconds(50);
}
ScizzorSteps = scizzorBotLatch;
}
scizzorOn = true;
dropScizzor();
scizzorOn = false;
Timer1.initialize(500000);
Timer1.attachInterrupt(interruptScizzor);
//wdt_enable(WDTO_8S);
Timer3.initialize(1000000);
Timer3.attachInterrupt(blinkDynamixel);
}
void blinkDynamixel()
{
if(!dynamixelLED)
{
payLoad[0] = 1;
payLoad[1] = 1; // One Packet
payLoad[2] = 5; //Length
payLoad[3] = 254;
payLoad[4] = 04;
payLoad[5] = 03;
payLoad[6] = 25;
payLoad[7] = 01;
dynamixelLED = !dynamixelLED;
}
else
{
payLoad[0] = 1;
payLoad[1] = 1; // One Packet
payLoad[2] = 5; //Length
payLoad[3] = 254;
payLoad[4] = 04;
payLoad[5] = 03;
payLoad[6] = 25;
payLoad[7] = 00;
dynamixelLED = !dynamixelLED;
}
updateDynamixels();
}
void packetTimeout() // Packet has timed out, reset packet buffer
{
i = 0;
for(int j = 0; j < BUFFER_SIZE; j++)
{
payLoad[j] = 0;
packetBuffer[j] = 0;
}
}
void dropScizzor()
{
digitalWrite(SCIZZOR_DIR_PIN,HIGH);
while(ScizzorSteps > 1)
{
//wdt_reset();
interruptScizzor();
delayMicroseconds(50);
}
}
void interruptScizzor()
{
if(scizzorOn)
{
digitalWrite(SCIZZOR_STEP_PIN,!digitalRead(SCIZZOR_STEP_PIN)); // Toggle pin
if(!digitalRead(SCIZZOR_DIR_PIN)) ScizzorSteps = ScizzorSteps + 1; // If set to raise lift, increment upwards
else ScizzorSteps = ScizzorSteps - 1;
}
if(ScizzorSteps == 1) scizzorOn = false;
if(ScizzorSteps == maxScizzorSteps -1) scizzorOn = false;
}
void overrideInterruptScizzor()
{
digitalWrite(SCIZZOR_STEP_PIN,!digitalRead(SCIZZOR_STEP_PIN)); // Toggle pin
if(!digitalRead(SCIZZOR_DIR_PIN)) ScizzorSteps = ScizzorSteps + 1; // If set to raise lift, increment upwards
else ScizzorSteps = ScizzorSteps - 1;
}
void manualScizzorCallibration()
{
if(payLoad[1] == 0x00) // Set to move downards with no limits
{
Timer1.attachInterrupt(overrideInterruptScizzor);
Timer1.setPeriod(600);
}
if(payLoad[1] == 0x01) // Reached the bottom, now increment upwards until the scizzor latch location is found
{
ScizzorSteps = 0;
digitalWrite(SCIZZOR_DIR_PIN,LOW);
while(digitalRead(REED_BOTTOM_PIN))
{
digitalWrite(SCIZZOR_STEP_PIN,!digitalRead(SCIZZOR_STEP_PIN));
delayMicroseconds(50);
ScizzorSteps = ScizzorSteps + 1;
} // Found the new unlatching point
unsigned long ScizzorUnlatch=ScizzorSteps;
digitalWrite(SCIZZOR_DIR_PIN,HIGH);
while(!digitalRead(REED_BOTTOM_PIN))
{
digitalWrite(SCIZZOR_STEP_PIN,!digitalRead(SCIZZOR_STEP_PIN));
delayMicroseconds(50);
ScizzorSteps = ScizzorSteps - 1;
}
client.write('V');
client.write('T');
client.write(0x09);
client.write(0x09);
x = 0xFF;
x = x&ScizzorUnlatch; // Return Unlatching point
z = ScizzorUnlatch >> 8;
p = ScizzorUnlatch >> 16;
o = ScizzorUnlatch >> 24;
client.write(o);
client.write(p);
client.write(z);
client.write(x);
x = x&ScizzorSteps; // Return latching point
z = ScizzorSteps >> 8;
p = ScizzorSteps >> 16;
o = ScizzorSteps >> 24;
client.write(o);
client.write(p);
client.write(z);
client.write(x);
Timer1.attachInterrupt(interruptScizzor); // Re-attach the correct interrupt for normal operation
Timer1.setPeriod(2000);
}
}
void readReturnData()
{
digitalWrite(DYNA_TOGGLE_PIN,LOW);
delay(DYNA_DELAY);
while(Serial3.available()>0)
{
bufReturn[itReturn] = Serial3.read();
itReturn++;
}
digitalWrite(DYNA_TOGGLE_PIN,HIGH);
}