void stagInterrupt(void)
{
g_stag.moveLegs();
float a, b, c;
for (int i = 0; i < 4; i++)
{
g_stag.getLeg(i, &a, &b, &c);
/*
puts("Leg ");
puti(i+1);
puts(" Value1: ");
puti(servoPulseOffset[i*3] + a*angleToPulseFactor);
puts(" Value2: ");
puti(servoPulseOffset[i*3 + 1] + b*angleToPulseFactor);
puts(" Value3: ");
puti(servoPulseOffset[i*3 + 2] + c*angleToPulseFactor);
putln();
*/
servoWrite(legPins[i*3], servoPulseOffset[i*3] + a*angleToPulseFactor);
servoWrite(legPins[i*3 + 1], servoPulseOffset[i*3 + 1] + b*angleToPulseFactor);
servoWrite(legPins[i*3 + 2], servoPulseOffset[i*3 + 2] + c*angleToPulseFactor);
}
//putln();
// Sensor feeds are sent from here
ROM_TimerIntClear(WTIMER0_BASE, TIMER_TIMA_TIMEOUT);
}
void Enriduino::start(){
if (Serial.available()){
serial[i++] = Serial.read();
}
if (serial[i-1]==126){
i=0;
pinmode();
Write();
Read();
AWrite();
ARead();
servo();
servoWrite();
}
}