Linear Stage in Android¶
This example demonstrates the initialization of a linear stage and then randomly generated moves between the limit switches. The GUI shows a graphic of the stage as it is moving, periodically asking PiMotion™ for its current position.
package com.example.pimotiondemo;
import android.support.v7.app.ActionBarActivity;
import android.os.AsyncTask;
import android.os.Bundle;
import android.os.Handler;
import android.view.Menu;
import android.view.MenuItem;
import android.view.View;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.EditText;
import android.widget.SeekBar;
import android.widget.SeekBar.OnSeekBarChangeListener;
import android.widget.ToggleButton;
public class MainActivity extends ActionBarActivity {
private SeekBar seekBar;
private Handler position_handler;
private ToggleButton toggleButton;
private EditText editText;
private Stage s;
private StageImageView view;
private int seekBarVal;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
seekBar = (SeekBar) findViewById(R.id.seekBar1);
editText = (EditText)findViewById(R.id.editText1);
toggleButton = (ToggleButton)findViewById(R.id.toggleButton1);
view = (StageImageView)findViewById(R.id.imageView1);
seekBarVal = 0;
position_handler = new Handler();
/*startup_task = (StartupTask) */new StartupTask().execute("dummy");
}
private Runnable position_runnable = new Runnable() {
@Override
public void run() {
try {
boolean doRandom = toggleButton.isChecked();
if (s.mtr_get_profile_finished() != 0) {
seekBar.setEnabled(!doRandom);
if (doRandom) {
s.move_to_random();
}
else {
int newSeekBarVal = seekBar.getProgress();
if (newSeekBarVal != seekBarVal) {
seekBarVal = newSeekBarVal;
long pos = (((s.right - s.left) * seekBarVal) / 100) + s.left;
s.move_to(pos, false);
}
}
}
long pos = s.mtr_get_actual_pos();
float percent = ((pos - s.left) * 100) / (s.right - s.left);
view.update_position((int)pos, (int) percent);
editText.setText(Long.toString(pos));
} catch (Exception e)
{
editText.setText(e.toString());
}
/* and here comes the "trick" */
position_handler.postDelayed(this, 100);
}
};
private class StartupTask extends AsyncTask<String, Void, String> {
@Override
protected String doInBackground(String... params) {
s = new Stage("pimotion00009999");
return "Done";
}
@Override
protected void onPostExecute(String result) {
editText.setText("");
position_handler.postDelayed(position_runnable, 100);
}
@Override
protected void onPreExecute() {
editText.setText("Initializing");
}
@Override
protected void onProgressUpdate(Void... values) {}
}
@Override
public boolean onCreateOptionsMenu(Menu menu) {
// Inflate the menu; this adds items to the action bar if it is present.
getMenuInflater().inflate(R.menu.main, menu);
return true;
}
@Override
public boolean onOptionsItemSelected(MenuItem item) {
// Handle action bar item clicks here. The action bar will
// automatically handle clicks on the Home/Up button, so long
// as you specify a parent activity in AndroidManifest.xml.
int id = item.getItemId();
if (id == R.id.action_settings) {
return true;
}
return super.onOptionsItemSelected(item);
}
}
package com.example.pimotiondemo;
import com.vena.Pilib;
import com.vena.Pilib.MtrSegment;
import com.vena.Pilib.TrapezoidalEntry;
import com.vena.Pilib.VelocityEntry;
import com.vena.Pilib.Trigger;
import com.vena.Pilib.ServoMode;
import java.util.Random;
public class Stage extends Pilib {
public class MtrConf {
public int elec_cycle_div = 50000;
public double kp = 1250.0;
public double ki = 10.0;
public double kd = 340.0;
public int ilim = 1000;
public int max_pid_output = 30000;
public int enc_counts_per_rot = 2048;
public int poles = 4;
public int phases = 3;
public int reset_current = 28000;
public int reset_wait_time_ms = 3000;
public int max_pos_error = 1500;
public int use_aux_enc_for_servo =0;
public int output_polarity = 0;
public int encoder_polarity = 0;
}
public Pilib p;
MtrConf conf;
long left;
long right;
TrapezoidalEntry[] entries;
MtrSegment[] segments;
int num_segments;
double last_vel;
Random rand;
public Stage(String address)
{
super(address,"password",0);
conf = new MtrConf();
rand = new Random();
entries = new TrapezoidalEntry[1];
entries[0] = new TrapezoidalEntry(0., 0, 5000., 8000., 8000.);
segments = new MtrSegment[3];
segments[0] = new MtrSegment();
segments[1] = new MtrSegment();
segments[2] = new MtrSegment();
// configure limit switches
gpio_set_direction(0xff, 0b11111110); // gpio 0 is output, all others input
gpio_set_pullup(0xff, 0b00000110); // pullup on gpio 1 & 2
gpio_set_value(0xff, 0x00); // set low
// get off limit switches to reset motor
if (gpio_get_value(0xff) != 0) {
mtr_set_pwr_enable(0);
print ("Move stage to the center...");
while (gpio_get_value(0xff) != 0) {
sleep(0.1);
}
sleep(2);
print("thanks!");
}
reset_motor();
last_vel = 0.0;
long start_pos = mtr_get_actual_pos();
find_endstop(-500, 0b00000010);
find_endstop(500, 0b00000000);
left = find_endstop(-50, 0b00000010);
move_to(start_pos, true);
find_endstop(500, 0b00000100);
find_endstop(-500, 0b00000000);
right = find_endstop(50, 0b00000100);
move_to(start_pos, true);
}
public void configure_motor()
{
mtr_set_mtr_type(0);
// set the pid and trajectory generator update rate
mtr_set_elec_cycle_div(conf.elec_cycle_div);
// set the pid gains (proportional, integral, derivitive)
mtr_set_kp(conf.kp);
mtr_set_ki(conf.ki);
mtr_set_kd(conf.kd);
mtr_set_ilim(conf.ilim);
// limit the output of the amplifiers
mtr_set_max_pid_output(conf.max_pid_output);
// set the number of encoder counts for one rotation of the motor
mtr_set_enc_counts_per_rot(conf.enc_counts_per_rot);
mtr_set_enc_polarity(conf.encoder_polarity);
// configure the number of poles the motor has
mtr_set_poles(conf.poles);
mtr_set_reset_current(conf.reset_current);
mtr_set_reset_wait_time_ms(conf.reset_wait_time_ms);
mtr_set_max_pos_error(conf.max_pos_error);
mtr_set_use_aux_enc_for_servo(conf.use_aux_enc_for_servo);
mtr_set_output_polarity(conf.output_polarity);
}
public void reset_motor()
{
configure_motor();
mtr_set_pwr_enable(1);
// configure the motor phase waveforms
mtr_write_phase_mem_default(conf.phases, conf.poles, conf.enc_counts_per_rot);
mtr_reset();
print ("Resetting...");
while (mtr_get_reset_state() != 0) {
sleep(0.1);
}
print("Finished resetting.");
}
public void stop()
{
long pos = mtr_get_actual_pos();
mtr_write_target_position(pos, 1);
}
public void start()
{
mtr_write_table(0, num_segments, segments);
mtr_start(0,Trigger.TRIG_SRC_NONE);
}
public void move_velocity(double velocity, double acceleration)
{
VelocityEntry profile[] = new VelocityEntry[1];
profile[0] = new VelocityEntry(0.0, velocity, acceleration);
num_segments = mtr_create_velocity_profile(last_vel, profile, ServoMode.SERVOMODE_POSITION, conf.elec_cycle_div, segments);
start();
last_vel = velocity;
}
public long find_endstop(int velocity, int gpio_val)
{
move_velocity(velocity, 1600);
while (mtr_get_motion_error() == 0)
{
long pos = mtr_get_actual_pos();
int val = gpio_get_value(0b00000110);
if (val == gpio_val)
{
stop();
mtr_set_motion_error(0);
return pos;
}
}
return 0;
}
public void move_to(long pos, boolean wait_for_complete)
{
long actual_pos = mtr_get_actual_pos();
if (actual_pos != pos) {
entries[0].position = pos;
num_segments = mtr_create_trapezoidal_profile(actual_pos, entries, conf.elec_cycle_div, segments);
start();
while (wait_for_complete && (mtr_get_profile_finished() == 0)) {
sleep(0.1);
}
}
}
public void move_to_random()
{
long pos = randlong(left + 50, right - 50);
move_to(pos, false);
}
public long randlong(long min, long max)
{
return rand.nextInt((int)((max - min) + 1)) + min;
}
public void print(String data)
{
System.out.println(data);
}
public void sleep(double sec)
{
int ms = (int)(sec * 1000.0);
try {
Thread.sleep(ms);
} catch(InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
public void print_temp()
{
double temperature = pidev_get_temperature();
System.out.format("The temperature is %.2f C\n", temperature);
}
}
package com.example.pimotiondemo;
import android.annotation.SuppressLint;
import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.AttributeSet;
import android.widget.ImageView;
public class StageImageView extends ImageView {
public String message;
public Bitmap slide_bmp;
public int percent;
public StageImageView(Context context, AttributeSet attributeSet) {
super(context, attributeSet);
message = "Hello";
percent = 0;
slide_bmp = BitmapFactory.decodeResource(getResources(),
R.drawable.slide_only_small);
}
public void process(String strImageFilePath) {
//doing some operation
invalidate();
}
public void update_position(int encoder, int stage_percent)
{
message = Integer.toString(encoder);
percent = stage_percent;
invalidate();
}
@SuppressLint("DrawAllocation")
@Override
protected void onDraw(Canvas canvas) {
super.onDraw(canvas);
int h = this.getHeight();
int w = this.getWidth();
int x = (int) (w * 0.0045 * percent);
int y = (int) (h * 0.20);
//message = Integer.toString(h) + ", " + Integer.toString(w);
Paint paint = new Paint();
paint.setColor(Color.RED);
paint.setTextSize(20);
float textWidth = paint.measureText(message);
canvas.drawText(message, (getWidth() - textWidth) / 2, 20, paint);
canvas.drawBitmap(slide_bmp, x, y, null);
}
}