I am working on creating a biped robot. I have function running in Processing that output motor positions to the servos on the robot. Currently I am using the Servo Firmata loaded onto my arduino uno. This works fine with the Standard Version of Processing 1.5.1. I am using a MacBook Pro. I have an initial successful walking gait, but I would like to add an accelerometer so I want to use the standard firmata. When the standard firmata is loaded onto the arduino the same program no longer functions. I have used a stand alone firmata tester application downloaded from the firmata website and I can control the servos and i/o pins fine.
I am not sure where the problem is. Is this common that the standard firmata does not work with servos? I have tried a different computer with different code and still it does not work.
The Processing sketch is as follows:
import processing.serial.*;
import cc.arduino.*;
Arduino arduino;
float y1f, y2f, y3f, y4f, y5f, y6f;
int y1i, y2i, y3i, y4i, y5i, y6i;
float x1;
float s = 4.0;
//Right Knee
float A1 = -60.0;//39.084;
float B1 = 10.0;//26.434;
float C1 = 6.00;//5.1762;
float D1 = 90.0;//109.2;
//Left Knee
float A2 = 60.00;//-38.612;
float B2 = 30.00;//5.3432;
float C2 = 6.000;//4.9554;
float D2 = 90.00;//90.392;
float A3 = 20.00;//90;//74.808;
float B3 = 0.1571;//-2.9442;
float C3 = 5.284;//2.142;//0.15928;
float D3 = 70.00;//-0.0021288;
float A4 = 20.00;//90;//74.808;
float B4 = 0.1571;//-2.9442;
float C4 = 5.284;//2.142;//3.142;//0.15928;
float D4 = 110.00;//-0.0021288;
float A5 = 20.00;
float B5 = 0.1571;
float C5 = 0;//3.142;
float D5 = 85.00;
float A6 = 15.00;
float B6 = 0.1571;
float C6 = 0;
float D6 = 90.00;
void setup() {
size(470, 200);
println(Arduino.list());
arduino = new Arduino(this, Arduino.list()[4], 57600);
// arduino.pinMode(14, Arduino.INPUT);
// arduino.pinMode(15, Arduino.INPUT);
arduino.pinMode(8, Arduino.SERVO);
arduino.pinMode(9, Arduino.SERVO);
arduino.pinMode(10, Arduino.SERVO);
arduino.pinMode(11, Arduino.SERVO);
arduino.pinMode(12, Arduino.SERVO);
arduino.pinMode(7, Arduino.SERVO);
}
void draw(){
line(1,250,50,250);
for(float i=1.0; i<41.0; i=i+s){
x1 = i;
y2f = A1*exp(-pow((x1-B1),2)/pow(C1,2))+D1;
y1f = A2*exp(-pow((x1-B2),2)/pow(C2,2))+D2;
y3f = A3*sin(B3*x1+C3)+D3;
y4f = A4*sin(B4*x1+C4)+D4;
y5f = A5*sin(B5*x1+C5)+D5;
y6f = A6*sin(B6*x1+C6)+D6;
//y3f = A3+B3*x1+C3*pow(x1,2)+D3*pow(x1,3);
// y4f = A4+B4*(-1*x1+41.000)+C4*pow((-x1+41.000),2)+D4*pow((-x1+41.000),3);
println("Y values");
println(y1f);
println(y2f);
println(y3f);
println(y4f);
println(y5f);
y1i = int(y1f);
y2i = int(y2f);
y3i = int(y3f);
y4i = int(y4f);
y5i = int(y5f);
y6i = int(y6f);
arduino.analogWrite(7, y6i);
delay(15);
arduino.analogWrite(8, y1i);
delay(15);
arduino.analogWrite(12, y2i);
delay(15);
arduino.analogWrite(9, y3i);
delay(15);
arduino.analogWrite(11, y4i);
delay(15);
arduino.analogWrite(10, y5i);
delay(15);
// delay(50);
y1i = -1*y1i+250;
y2i = -1*y2i+250;
y3i = -1*y3i+250;
y4i = -1*y4i+250;
point(x1,y1i);
point(x1,y2i);
point(x1,y3i);
point(x1,y4i);
}
}