GPS C# Class Tiny CLR for Fez Panda II or Netduino

#include <Servo.h>

String inputString = ""; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
int com_state = 0;
int PackCount = 0;
char Packet[11] = {0, };

long Cooling = 0;
long PumpSpeed = 0;

Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;

#define MAX_CHAN 8
int AnIn[MAX_CHAN] = {0, };

unsigned long PPMtout = 20000;

char buf[300] = {0, };

long mS = 0;

void setup() {

pinMode(13, OUTPUT);

Serial.begin(115200);

// reserve 200 bytes for the inputString:
inputString.reserve(200);

Servo1.attach(8);
Servo2.attach(9);
Servo3.attach(10);
Servo4.attach(11);

}

void loop() {

AnIn[0] = analogRead(A0);
AnIn[1] = analogRead(A1);
AnIn[2] = analogRead(A2);
AnIn[3] = analogRead(A3);
AnIn[4] = analogRead(A4);
AnIn[5] = analogRead(A5);
AnIn[6] = analogRead(A6);
AnIn[7] = analogRead(A7);

/* RAZU-2 Six turn
This device has a gain of 55mV per amp giving a 36 Amp working range,
and a resolution of 11.3777 mA per bit with the maximum output at 5 volts
which is 1024 bits in the Arduino. By the way I should point out that
these devices on a 5 volt rail will sit around 2.5 volts with no current
flowing, so at a 2 Volt delta the out put should be around 4.5 V
*/

mS = micros();

//Calc amps
long Amps = (long)map((AnIn[1] - 512), 0, 512, 0, 36);

//Calculate cooling temp RTC on AnIn[0]
int cerr = ((int)600 - (int)AnIn[0]);

//Integrate temparature
Cooling += cerr;

if(Cooling < 1000) Cooling = 1000;
else if(Cooling > 2000) Cooling = 2000;

//Use the temperature integrator plus the motor current
//36 Amps gives us 360 bits
PumpSpeed = (long)Cooling + (Amps * 10);

//Update the outputs

Servo1.writeMicroseconds((int)PumpSpeed);

long Cyc_mS = (micros() - mS);

// print the string when a newline arrives:
if (stringComplete) {

//Servo2.writeMicroseconds((int)Packet[4]);
Servo2.write((int)Packet[5]);

sprintf(buf, "AnIn[0]:%d,AnIn[1]:%d,CoolingError:%d,Cooling:%d,MotorAmps:%d,Cyc_mS:%d", AnIn[0], AnIn[1], cerr, (int)Cooling, (int)Amps, (int)Cyc_mS );

Serial.println(buf);

stringComplete = false;
}

//ALLOW SERVOS TO UPDATE
delayMicroseconds(20000);

Cyc_mS = (micros() - mS);

}

/*
SerialEvent occurs whenever a new data comes in the
hardware serial RX. This routine is run between each
time loop() runs, so using delay inside loop can delay
response. Multiple bytes of data may be available.
*/
void serialEvent() {

while (Serial.available()) {

// get the new byte:
byte inChar = (byte)Serial.read();

switch(com_state)
{
case 0:

digitalWrite(13, HIGH);

if(inChar == 85){
Packet[0] = inChar;
PackCount = 1;
com_state = 1;
}
break;

case 1:

if(PackCount < 11)
Packet[PackCount++] = inChar;
else
{
PackCount = 0;

int sum = 0;
for (int i = 0; i < 9; i++)
sum += (int)Packet[i];

byte Chk = (byte)(sum % 256);

//sprintf(buf, "LC=%d\nRC=%d", (int)Chk, (int)Packet[10] );

//Serial.println(buf);

if(Chk == Packet[10])
{
digitalWrite(13, LOW);
stringComplete = true;
com_state = 0;

}
}
break;
}

}
}