Bueno, a la espera de tus correcciones, este es mi aporte.
Numerosos errores por doquier.
Muchas variables doblemente definidas. Si defines por ejemplo
float traveled_m1;
float traveled_m2;
no puedes en el setup definir esto
float traveled_m1=0;
float traveled_m2=0;
Porque no? Por que transformas a las variables definidas en el setup en locales.
Si quieres asignarle valor puedes hacerlo como hice yo o simplemente sin el float en el setup.
Muchas repeticiones, que borre y detalles por acá y alla.
A ver como funciona.
#include <math.h>
float error_m1;
float cumplimiento_m1;
float correccion_m1;
float error_m2;
float cumplimiento_m2;
float correccion_m2;
float refpos_m1[31] = {25.0, 23.7, 22.3, 21.0, 19.6, 18.3, 16.9, 15.6, 14.2, 12.9, 11.5, 10.2, 8.8, 7.4, 6.1, 4.7, 3.4, 2.0, 0.7, -0.7, -2.0, 0.0, 4.0, 8.0, 12.0, 16.0, 20.0, 23.0, 24.0, 25.0, 25.0};
float travel_m1[31] = {0.0, -1.3, -1.4, -1.3, -1.4, -1.3, -1.4, -1.3, -1.4, -1.3, -1.4, -1.3, -1.4, -1.4, -1.3, -1.4, -1.3, -1.4, -1.3, -1.4, -1.3, 2.0, 4.0, 4.0, 4.0, 4.0, 4.0, 3.0, 1.0, 1.0, 0.0};
float refpos_m2[31] = {10.0, 8.7, 7.3, 5.2, 3.2, 1.1, -1.0, -3.1, -5.1, -7.2, -9.3, -11.4, -13.4, -15.5, -17.6, -19.6, -21.7, -23.8, -25.9, -27.9, -30.0, -30.0, -38.0, -46.0, -50.0, -46.0, -31.0, -15.0, -1.0, 7.0, 10.0};
float travel_m2[31] = {0.0, -1.3, -1.4, -2.1, -2.0, -2.1, -2.1, -2.1, -2.0, -2.1, -2.1, -2.1, -2.0, -2.1, -2.1, -2.0, -2.1, -2.1, -2.1, -2.0, -2.1, 0.0, -8.0, -8.0, -4.0, 4.0, 15.0, 16.0, 14.0, 8.0, 3.0};
// lectura directa de acelerometros
float m1xval = 0;
float m2xval = 0;
//angulo directo del acelerometro ya con centrado
float degreePos_m1 = 0;
float degreePos_m2 = 0;
//relación de aceleracion en x / radio
double rel_m1_xr = 0;
double rel_m2_xr = 0;
float traveled_m1 = 0;
float traveled_m2 = 0;
void setup() {
// Motor_1 control pin initiate;
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(9, OUTPUT);
// Motor_2 control pin initiate;
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(10, OUTPUT);
analogReference(EXTERNAL);
Serial.begin(57600);
//Enable the Motor Shield output;
pinMode(6, OUTPUT);
digitalWrite(6, HIGH);
}
void loop() {
//Que quieres que haga? levantarme, sentarme, caminar, detenerme
//si es caminar
//1° Confirmar Posición vertical
//listo para caminar?
//1° Boton o señal de entrada SI
//2° Dar medio paso inicial
//3° iniciar caminata
//circulo para paso de 3 seg de mi peli 2
for (int i=0; i <= 31; i++){
//entradas de acelerometro
m1xval = analogRead(0);
m2xval = analogRead(1);
m1xval = map(m1xval, 0, 1023, 500, -500);
m2xval = map(m2xval, 0, 1023, 500, -500);
//ajustes de centrado por offset
m1xval = m1xval - 10; //ajuste de cero G
m2xval = m2xval - 10; //ajuste de cero G
//calculo de relacion entre lectura corregida /maximo a 1G
rel_m1_xr =((double)m1xval / 238);
rel_m2_xr =((double)m2xval / 238);
//calculo de angulos reales
degreePos_m1=asin(rel_m1_xr); //Calculo de angulo M1
degreePos_m1=degreePos_m1*(57.2958);
degreePos_m2=asin(rel_m2_xr); //Calculo de angulo M2
degreePos_m2=degreePos_m2*(57.2958);
//Calculos para corrector de factor de velocidad
error_m1 = refpos_m1[i] ; //- degreePos_m1;
traveled_m1 = travel_m1[i] - error_m1 ; //cambio angular realizado en paso anterior m1
cumplimiento_m1 = traveled_m1 / travel_m1[i];
correccion_m1 = 1 / cumplimiento_m1;
error_m2 = refpos_m2[i] - degreePos_m2;
traveled_m2 = travel_m2[i] - error_m2 - traveled_m1; //cambio angular realizado en rodilla de paso anterior
cumplimiento_m2 = traveled_m2 / ((travel_m2[i])- (travel_m1[i]));
correccion_m2 = 1 / cumplimiento_m2;
//speed in motor_1 is going to be (constant1 * travel_m1(i) * (1+ correccion_m1)
//speed in motor_2 is going to be (constant2 * travel_m2[i](i) * (1+ correccion_m2)
analogWrite(9, 127); // set the motor_1 speed ;
digitalWrite(4, HIGH);
digitalWrite(5, LOW); // Set the rotation of motor_1
analogWrite(10, 127); // set the motor_2 speed ;
digitalWrite(7, LOW);
digitalWrite(8, HIGH); // Set the rotation of motor_1
Serial.write("valorM1x=");
Serial.print(m1xval);
Serial.write("\n");
Serial.write("valorM2x=");
Serial.print(m2xval);
Serial.write("\n");
Serial.write("Angulo_en_M1=");
Serial.print(degreePos_m1);
Serial.write("\n");
Serial.write("Angulo_en_M2=");
Serial.print(degreePos_m2);
Serial.write("\n");
delay(500);
}
}