Paso 12: Código principal...
Este es el código principal... Puedo enviar un archivo ZIP con todo el proyecto Java a petición... por favor enviar solicitud a joebotics
CÓDIGO PRINCIPAL:
paquete ioio.examples.simple;
LEY DE EQUILIBRIO 4 + MOGA 2
Import ioio.lib.api.PwmOutput;
Import ioio.lib.api.exception.ConnectionLostException;
Import ioio.lib.util.BaseIOIOLooper;
Import ioio.lib.util.IOIOLooper;
Import ioio.lib.util.android.IOIOActivity;
Import android.os.Bundle;
android.widget.TextView importación;
Import android.hardware.Sensor;
Import android.hardware.SensorEvent;
Import android.hardware.SensorEventListener;
Import android.hardware.SensorManager;
Import android.widget.SeekBar;
Import com.bda.controller.Controller;
Import com.bda.controller.ControllerListener;
Import com.bda.controller.KeyEvent;
Import com.bda.controller.MotionEvent;
Import com.bda.controller.StateEvent;
pública clase que ioiosimpleapp extiende IOIOActivity implemente SensorEventListener {}
privado mSensorManager SensorManager;
privado mRotVectSensor Sensor;
privado float [] orientationVals = new float [3];
privado float [] mRotationMatrix = new float [16];
privado TextView textView_Current_Angle;
privado TextView textView_Tilt_adjuster;
privado TextView textView_kP_adjuster;
privado TextView textView_kI_adjuster;
privado TextView textView_kD_adjuster;
seekBar_Tilt_adjuster privado de SeekBar;
seekBar_kP_adjuster privado de SeekBar;
seekBar_kI_adjuster privado de SeekBar;
seekBar_kD_adjuster privado de SeekBar;
Controlador mController = null;
final mListener de ExampleControllerListener = new ExampleControllerListener();
mPlayer ExamplePlayer final = new ExamplePlayer (0.0f, 1.0f, 0.0f);
público void onCreate (Bundle savedInstanceState) {}
mController = Controller.getInstance(this);
mController.init();
mController.setListener (mListener, null);
super.onCreate(savedInstanceState);
setContentView(R.layout.main);
mSensorManager = getSystemService(SENSOR_SERVICE) (SensorManager);
mRotVectSensor=mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
textView_Current_Angle = (TextView) findViewById(R.id.TextView_CurrentAngle_Value);
textView_Tilt_adjuster = (TextView) findViewById(R.id.TextView_Tilt_adjusterValue);
seekBar_Tilt_adjuster = findViewById(R.id.SeekBar_Tilt_adjuster) (SeekBar);
textView_kP_adjuster = (TextView) findViewById(R.id.TextView_kP_adjusterValue);
seekBar_kP_adjuster = findViewById(R.id.SeekBar_kP_adjuster) (SeekBar);
textView_kI_adjuster = (TextView) findViewById(R.id.TextView_kI_adjusterValue);
seekBar_kI_adjuster = findViewById(R.id.SeekBar_kI_adjuster) (SeekBar);
textView_kD_adjuster = (TextView) findViewById(R.id.TextView_kD_adjusterValue);
seekBar_kD_adjuster = findViewById(R.id.SeekBar_kD_adjuster) (SeekBar);
seekBar_Tilt_adjuster.setProgress(500);
seekBar_kP_adjuster.setProgress(0);
seekBar_kI_adjuster.setProgress(0);
seekBar_kD_adjuster.setProgress(0);
enableUi(false);
}
clase pública ExampleControllerListener implementa ControllerListener {}
público vacío onKeyEvent (KeyEvent evento) {}
interruptor (event.getKeyCode()) {}
caso KeyEvent.KEYCODE_BUTTON_X:
seekBar_Tilt_adjuster.setProgress(seekBar_Tilt_adjuster.getProgress() - 2);
rotura;
caso KeyEvent.KEYCODE_BUTTON_B:
seekBar_Tilt_adjuster.setProgress(seekBar_Tilt_adjuster.getProgress() + 2);
rotura;
caso KeyEvent.KEYCODE_BUTTON_Y:
seekBar_kP_adjuster.setProgress(seekBar_kP_adjuster.getProgress() + 2);
rotura;
caso KeyEvent.KEYCODE_BUTTON_A:
seekBar_kP_adjuster.setProgress(seekBar_kP_adjuster.getProgress() - 2);
rotura;
caso KeyEvent.KEYCODE_DPAD_UP:
seekBar_kI_adjuster.setProgress(seekBar_kI_adjuster.getProgress() + 2);
rotura;
caso KeyEvent.KEYCODE_DPAD_DOWN:
seekBar_kI_adjuster.setProgress(seekBar_kI_adjuster.getProgress() - 2);
rotura;
caso KeyEvent.KEYCODE_DPAD_RIGHT:
seekBar_kD_adjuster.setProgress(seekBar_kD_adjuster.getProgress() + 2);
rotura;
caso KeyEvent.KEYCODE_DPAD_LEFT:
seekBar_kD_adjuster.setProgress(seekBar_kD_adjuster.getProgress() - 2);
rotura;
}
}
público vacío onMotionEvent (MotionEvent event) {}
mPlayer.mAxisX = event.getAxisValue(MotionEvent.AXIS_X);
mPlayer.mAxisY = event.getAxisValue(MotionEvent.AXIS_Y);
mPlayer.mAxisZ = event.getAxisValue(MotionEvent.AXIS_Z);
mPlayer.mAxisRZ = event.getAxisValue(MotionEvent.AXIS_RZ);
}
onStateEvent público void (actualnotificaciones event) {}
interruptor (event.getState()) {}
caso StateEvent.STATE_CONNECTION:
mPlayer.mConnection = event.getAction();
rotura;
}
}
}
clase ExamplePlayer {}
Static final float DEFAULT_SCALE = 4.0f;
Static final float DEFAULT_X = 0.0f;
Static final float DEFAULT_Y = 0.0f;
Boolean gotPadVersion = false;
mConnection público int = StateEvent.ACTION_DISCONNECTED;
público int mControllerVersion = StateEvent.STATE_UNKNOWN;
público int mButtonA = KeyEvent.ACTION_UP;
público int mButtonB = KeyEvent.ACTION_UP;
público int mButtonX = KeyEvent.ACTION_UP;
público int mButtonY = KeyEvent.ACTION_UP;
público int DpadUp = KeyEvent.ACTION_UP;
público int DpadDown = KeyEvent.ACTION_UP;
público int DpadLeft = KeyEvent.ACTION_UP;
público int DpadRight = KeyEvent.ACTION_UP;
mAxisX flotador público = 0.0f;
mAxisY flotador público = 0.0f;
mAxisZ flotador público = 0.0f;
mAxisRZ flotador público = 0.0f;
final flotante Señor;
final flotante mG;
final flotante mB;
Float mScale = DEFAULT_SCALE;
flotador mX = DEFAULT_X;
flotador de mi = DEFAULT_Y;
ExamplePlayer público (float r, float g, float b) {}
mR = r;
mG = g;
mB = b;
}
}
Lanzadera clase extiende {BaseIOIOLooper}
privada PwmOutput LeftWheel;
privada PwmOutput RightWheel;
privada PwmOutput LeftArm;
privado PwmOutput RightArm;
int privados currentAngle;
privada int previousAngle;
privada int P = 0;
privada int I = 0;
privada int D = 0;
privada int PID = 0;
privada int D_delta;
privada int I_delta;
público int seekbar_tilt_adjuster_value = 500;
privada int seekbar_kP_adjuster_value = 0;
privada int seekbar_kI_adjuster_value = 0;
privada int seekbar_kD_adjuster_value = 0;
lanza de público void setup() {} ConnectionLostException
LeftArm = ioio_.openPwmOutput (13, 50);
RightArm = ioio_.openPwmOutput (12, 50);
LeftWheel = ioio_.openPwmOutput (10, 100);
RightWheel = ioio_.openPwmOutput (11, 100);
enableUi(true);
}
público void loop() lanza ConnectionLostException, InterruptedException {}
seekbar_tilt_adjuster_value = seekBar_Tilt_adjuster.getProgress() - 500;
seekbar_kP_adjuster_value = seekBar_kP_adjuster.getProgress();
seekbar_kI_adjuster_value = seekBar_kI_adjuster.getProgress();
seekbar_kD_adjuster_value = seekBar_kD_adjuster.getProgress();
currentAngle = (Math.round (orientationVals [1] * 100)) + seekbar_tilt_adjuster_value - Math.round(mPlayer.mAxisY*10) - Math.round(mPlayer.mAxisRZ*10);
currentAngle = (Math.round (orientationVals [1] * 100)) + seekbar_tilt_adjuster_value;
D_delta = currentAngle - previousAngle;
I_delta = I_delta + D_delta;
P = (seekbar_kP_adjuster_value * currentAngle) 2000;
I = (seekbar_kI_adjuster_value * I_delta) / 4000;
D = (seekbar_kD_adjuster_value * D_delta) / 4000;
Si (I < -100) {}
I = -100;
} else if (I > 100) {}
I = 100;
// }
PID = P + I + D;
previousAngle = currentAngle;
RightArm.setPulseWidth (1590 - Math.round(mPlayer.mAxisY*900));
LeftArm.setPulseWidth (1540 + Math.round(mPlayer.mAxisRZ*900));
RightWheel.setPulseWidth (1500 + Math.round(mPlayer.mAxisY*35) + PID);
LeftWheel.setPulseWidth (1502 - Math.round(mPlayer.mAxisRZ*35) - PID);
LeftWheel.setPulseWidth (1502 - PID);
RightWheel.setPulseWidth(1500 + PID);
setText_current_angle(Integer.toString(currentAngle));
setText_tilt_adjuster(Integer.toString(seekbar_tilt_adjuster_value));
setText_kP_adjuster(Integer.toString(seekbar_kP_adjuster_value));
setText_kI_adjuster(Integer.toString(seekbar_kI_adjuster_value));
setText_kD_adjuster(Integer.toString(seekbar_kD_adjuster_value));
setText_current_angle(Integer.toString(currentAngle));
setText_tilt_adjuster(Integer.toString(PID));
setText_kP_adjuster(Integer.toString(P));
setText_kI_adjuster(Integer.toString(I));
setText_kD_adjuster(Integer.toString(D));
}
público void disconnected() {}
enableUi(false);
}
}
protegida IOIOLooper createIOIOLooper() {
volver a Looper() nuevo;
}
{} privado void enableUi (boolean habilitar el final)
runOnUiThread (new Runnable() de {}
público void run() {}
}
});
}
setText_current_angle privado void (final String str) {}
runOnUiThread (new Runnable() de {}
público void run() {}
textView_Current_Angle.setText(str);
}
});
}
setText_tilt_adjuster privado void (final String str) {}
runOnUiThread (new Runnable() de {}
público void run() {}
textView_Tilt_adjuster.setText(str);
}
});
}
setText_kP_adjuster privado void (final String str) {}
runOnUiThread (new Runnable() de {}
público void run() {}
textView_kP_adjuster.setText(str);
}
});
}
setText_kI_adjuster privado void (final String str) {}
runOnUiThread (new Runnable() de {}
público void run() {}
textView_kI_adjuster.setText(str);
}
});
}
setText_kD_adjuster privado void (final String str) {}
runOnUiThread (new Runnable() de {}
público void run() {}
textView_kD_adjuster.setText(str);
}
});
}
público vacío onSensorChanged (SensorEvent evento)
{
if(Event.sensor.GetType()==sensor.TYPE_ROTATION_VECTOR)
{
SensorManager.getRotationMatrixFromVector(mRotationMatrix,event.values);
SensorManager.remapCoordinateSystem (mRotationMatrix,SensorManager.AXIS_X, SensorManager.AXIS_Z, mRotationMatrix);
SensorManager.getOrientation (mRotationMatrix, orientationVals);
orientationVals[1]=(float)Math.toDegrees(orientationVals[1]);
seekBar_kD_adjuster.setProgress(Math.round((orientationVals[1]*4)+500));
}
}
público vacío onAccuracyChanged (el sensor, exactitud de int) {}
}
protegido {} void onResume()
super.onResume();
registrar esta clase como oyente para la orientación y
Sensores acelerómetro
mSensorManager.registerListener (esto,
mRotVectSensor,
10000);
mController.onResume();
mPlayer.mConnection = mController.getState(Controller.STATE_CONNECTION);
mPlayer.mControllerVersion = mController.getState(Controller.STATE_CURRENT_PRODUCT_VERSION); Versión del controlador
mPlayer.mButtonA = mController.getKeyCode(Controller.KEYCODE_BUTTON_A);
mPlayer.mButtonB = mController.getKeyCode(Controller.KEYCODE_BUTTON_B);
mPlayer.mButtonX = mController.getKeyCode(Controller.KEYCODE_BUTTON_X);
mPlayer.mButtonY = mController.getKeyCode(Controller.KEYCODE_BUTTON_Y);
mPlayer.DpadUp = mController.getKeyCode(Controller.KEYCODE_DPAD_UP);
mPlayer.DpadDown = mController.getKeyCode(Controller.KEYCODE_DPAD_DOWN);
mPlayer.DpadLeft = mController.getKeyCode(Controller.KEYCODE_DPAD_LEFT);
mPlayer.DpadRight = mController.getKeyCode(Controller.KEYCODE_DPAD_RIGHT);
mPlayer.mAxisY = mController.getAxisValue(Controller.AXIS_Y);
mPlayer.mAxisRZ = mController.getAxisValue(Controller.AXIS_RZ);
}
protegido void onPause() {}
anular el registro de escucha
super.onPause();
mSensorManager.unregisterListener(this);
mController.onPause();
}
protegido {} void onDestroy()
mController.exit();
super.onDestroy();
}
}
// ----------------- ROBOT DIAGRAM ----------------------------- //
//
// _________
// | Frente |
// | Frente a |
// | Teléfono |
// ----------
// | |
// =========
LeftArm = 13 || ===| |===|| RightArm = 12
// || ===| |===||
// || | | ||
// || | | ||
// | |
// | |
// || | | ||
LeftWheel = 10 || =============|| RightWheel = 11
// || ||
//
//
