Baldroid v3 balansera Robot med Actobotics delar och IOIO-OTG (12 / 12 steg)

Steg 12: Main koden...

Här är den viktigaste koden... Jag kan skicka en ZIP-fil med hela Java projektet på begäran... Skicka ansökan till joebotics

MAIN-KOD:

paketet ioio.examples.simple;

BALANCE ACT 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;

import android.widget.TextView;

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;

allmän klass IOIOSimpleApp sträcker sig IOIOActivity implementerar SensorEventListener {

privat SensorManager mSensorManager;

privat Sensor mRotVectSensor;

privat float [] orientationVals = ny float [3].

privat float [] mRotationMatrix = ny float [16].

privat TextView textView_Current_Angle;

privat TextView textView_Tilt_adjuster;

privat TextView textView_kP_adjuster;

privat TextView textView_kI_adjuster;

privat TextView textView_kD_adjuster;

privat SeekBar seekBar_Tilt_adjuster;

privat SeekBar seekBar_kP_adjuster;

privat SeekBar seekBar_kI_adjuster;

privat SeekBar seekBar_kD_adjuster;

Controller mController = null;

sista ExampleControllerListener mListener = ny ExampleControllerListener();

sista ExamplePlayer mPlayer = nya ExamplePlayer (0.0f, 1.0f, 0.0f);

public void onCreate (bunt savedInstanceState) {

mController = Controller.getInstance(this);

mController.init();

mController.setListener (mListener, null);

super.onCreate(savedInstanceState);

setContentView(R.layout.main);

mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);

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 = (SeekBar) findViewById(R.id.SeekBar_Tilt_adjuster);

textView_kP_adjuster = (TextView) findViewById(R.id.TextView_kP_adjusterValue);

seekBar_kP_adjuster = (SeekBar) findViewById(R.id.SeekBar_kP_adjuster);

textView_kI_adjuster = (TextView) findViewById(R.id.TextView_kI_adjusterValue);

seekBar_kI_adjuster = (SeekBar) findViewById(R.id.SeekBar_kI_adjuster);

textView_kD_adjuster = (TextView) findViewById(R.id.TextView_kD_adjusterValue);

seekBar_kD_adjuster = (SeekBar) findViewById(R.id.SeekBar_kD_adjuster);

seekBar_Tilt_adjuster.setProgress(500);

seekBar_kP_adjuster.setProgress(0);

seekBar_kI_adjuster.setProgress(0);

seekBar_kD_adjuster.setProgress(0);

enableUi(false);

}

allmän klass ExampleControllerListener implementerar ControllerListener {

public void onKeyEvent (KeyEvent händelse) {

Switch (event.getKeyCode()) {

fall KeyEvent.KEYCODE_BUTTON_X:

seekBar_Tilt_adjuster.setProgress(seekBar_Tilt_adjuster.getProgress() - 2);

bryta;

fall KeyEvent.KEYCODE_BUTTON_B:

seekBar_Tilt_adjuster.setProgress(seekBar_Tilt_adjuster.getProgress() + 2);

bryta;

fall KeyEvent.KEYCODE_BUTTON_Y:

seekBar_kP_adjuster.setProgress(seekBar_kP_adjuster.getProgress() + 2);

bryta;

fall KeyEvent.KEYCODE_BUTTON_A:

seekBar_kP_adjuster.setProgress(seekBar_kP_adjuster.getProgress() - 2);

bryta;

fall KeyEvent.KEYCODE_DPAD_UP:

seekBar_kI_adjuster.setProgress(seekBar_kI_adjuster.getProgress() + 2);

bryta;

fall KeyEvent.KEYCODE_DPAD_DOWN:

seekBar_kI_adjuster.setProgress(seekBar_kI_adjuster.getProgress() - 2);

bryta;

fall KeyEvent.KEYCODE_DPAD_RIGHT:

seekBar_kD_adjuster.setProgress(seekBar_kD_adjuster.getProgress() + 2);

bryta;

fall KeyEvent.KEYCODE_DPAD_LEFT:

seekBar_kD_adjuster.setProgress(seekBar_kD_adjuster.getProgress() - 2);

bryta;

}

}

public void onMotionEvent (MotionEvent händelse) {

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);

}

public void onStateEvent (StateEvent händelse) {

Switch (event.getState()) {

fall StateEvent.STATE_CONNECTION:

mPlayer.mConnection = event.getAction();

bryta;

}

}

}

allmän klass ExamplePlayer {

statisk slutliga float DEFAULT_SCALE = 4.0f;

statisk slutliga float DEFAULT_X = 0.0f;

statisk slutliga float DEFAULT_Y = 0.0f;

booleska gotPadVersion = false;

Public int mConnection = StateEvent.ACTION_DISCONNECTED;

Public int mControllerVersion = StateEvent.STATE_UNKNOWN;

Public int mButtonA = KeyEvent.ACTION_UP;

Public int mButtonB = KeyEvent.ACTION_UP;

Public int mButtonX = KeyEvent.ACTION_UP;

Public int mButtonY = KeyEvent.ACTION_UP;

Public int DpadUp = KeyEvent.ACTION_UP;

Public int DpadDown = KeyEvent.ACTION_UP;

Public int DpadLeft = KeyEvent.ACTION_UP;

Public int DpadRight = KeyEvent.ACTION_UP;

offentliga flyta mAxisX = 0.0f;

offentliga flyta mAxisY = 0.0f;

offentliga flyta mAxisZ = 0.0f;

offentliga flyta mAxisRZ = 0.0f;

slutliga flyta Herr;

slutliga float mG;

slutliga float mB;

flyta mScale = DEFAULT_SCALE;

flyta mX = DEFAULT_X;

float min = DEFAULT_Y;

offentliga ExamplePlayer (flyta r, flyta g, flyta b) {

Herr = r;

mG = g;

mB = b;

}

}

offentliga klassen Looper utökar BaseIOIOLooper {

privat PwmOutput LeftWheel;

privat PwmOutput RightWheel;

privat PwmOutput LeftArm;

privat PwmOutput RightArm;

privata int currentAngle;

privata int previousAngle;

privata int P = 0;

privata int jag = 0;

privata int D = 0;

privata int PID = 0;

privata int D_delta;

privata int I_delta;

Public int seekbar_tilt_adjuster_value = 500.

privata int seekbar_kP_adjuster_value = 0;

privata int seekbar_kI_adjuster_value = 0;

privata int seekbar_kD_adjuster_value = 0;

public void setup() kastar ConnectionLostException {

LeftArm = ioio_.openPwmOutput (13, 50);

RightArm = ioio_.openPwmOutput (12, 50);

LeftWheel = ioio_.openPwmOutput (10, 100);

RightWheel = ioio_.openPwmOutput (11, 100);

enableUi(true);

}

public void loop() kastar 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;

Jag = (seekbar_kI_adjuster_value * I_delta) / 4000;

D = (seekbar_kD_adjuster_value * D_delta) / 4000;

om (jag < -100) {

Jag = -100;

} else om (jag > 100) {

Jag = 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));

}

public void disconnected() {

enableUi(false);

}

}

skyddade IOIOLooper createIOIOLooper() {

returnera nya Looper();

}

privata void enableUi (slutliga booleska aktivera) {

runOnUiThread (nya Runnable() {

public void run() {

}

});

}

privata void setText_current_angle (sista strängen str) {

runOnUiThread (nya Runnable() {

public void run() {

textView_Current_Angle.setText(str);

}

});

}

privata void setText_tilt_adjuster (sista strängen str) {

runOnUiThread (nya Runnable() {

public void run() {

textView_Tilt_adjuster.setText(str);

}

});

}

privata void setText_kP_adjuster (sista strängen str) {

runOnUiThread (nya Runnable() {

public void run() {

textView_kP_adjuster.setText(str);

}

});

}

privata void setText_kI_adjuster (sista strängen str) {

runOnUiThread (nya Runnable() {

public void run() {

textView_kI_adjuster.setText(str);

}

});

}

privata void setText_kD_adjuster (sista strängen str) {

runOnUiThread (nya Runnable() {

public void run() {

textView_kD_adjuster.setText(str);

}

});

}

public void onSensorChanged (SensorEvent händelse)

{

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));

}

}

public void onAccuracyChanged (Sensor sensor, int noggrannhet) {

}

skyddade void onResume() {

super.onResume();

registrera den här klassen som avlyssnare för orientering och

accelerationsmätare sensorer

mSensorManager.registerListener (detta,

mRotVectSensor,

10000);

mController.onResume();

mPlayer.mConnection = mController.getState(Controller.STATE_CONNECTION);

mPlayer.mControllerVersion = mController.getState(Controller.STATE_CURRENT_PRODUCT_VERSION); Få styrenhet version

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);

}

skyddade void onPause() {

avregistrera lyssnaren

super.onPause();

mSensorManager.unregisterListener(this);

mController.onPause();

}

skyddade void onDestroy() {

mController.exit();

super.onDestroy();

}

}

// ----------------- ROBOT DIAGRAM ----------------------------- //

//

// _________

// | Främre |

// | Inför |

// | Telefon |

// ----------

// | |

// =========

LeftArm = 13 || ===| |===|| RightArm = 12

// || ===| |===||

// || | | ||

// || | | ||

// | |

// | |

// || | | ||

LeftWheel = 10 || =============|| RightWheel = 11

// || ||

//

//