DR HACK

Davide Rosa Hack

itenes
29553
OggiOggi52
IeriIeri65
Questa settimanaQuesta settimana294
Questo MeseQuesto Mese827
TuttoTutto29553
Utenti 0
Visitatori 2

RoboZak

In questa sezione viene fatto un esempio di un'app per il progetto IOIO RoboZak

Classe per il controllo dei motori:


package robozak.examples.simple;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;

import ioio.lib.api.Uart;

public class Motore {
	int min_pos;
	int max_pos;
	int cur_pos ;
	int tmr_out ;
	int motor_id;
	
	int buff_siz = 7;	
	byte[] buff_out = new byte[buff_siz];
	byte[] buff_inp = new byte[buff_siz];
	
	InputStream uart_rx;
	OutputStream uart_tx;
	 
	public Motore(){
		 min_pos = 600;
		 max_pos = 2400;
		 cur_pos = (min_pos + max_pos)/2;
		 tmr_out = 100;
		 motor_id = 0;
	 }

	public Motore(Uart urt, int id, int speed, StringBuilder msg) throws IOException{
		 min_pos = 600;
		 max_pos = 2400;
		 cur_pos = (min_pos + max_pos)/2;
		 tmr_out = 100;
		 motor_id = 0;
		 
		 set_uart(urt);
     	 set_motor_id(id);
     	 msg.append(Motor_off() + "- ");
     	 msg.append(Motor_speed(speed));
     	 
	 }
	
    byte Tx_chk() {
    	return (byte)(256 - (int)((int)((int)buff_out[0] + (int)buff_out[1] + (int)buff_out[2] + (int)buff_out[3]) % 256) % 256);
    }

    byte Rx_chk() {
    	return (byte)(256 - (int)((int)((int)buff_inp[0] + (int)buff_inp[1] + (int)buff_inp[2] + (int)buff_inp[3]) % 256) % 256);
    }
    
    String Motor_send() throws IOException{
        StringBuilder sb = new StringBuilder();
        
        buff_out[4] = Tx_chk();
    	buff_out[5] = (byte)0x00;
    	buff_out[6] = (byte)0x00;  
    	
    	sb.append(flush_rx());
    	
    	uart_tx.write(buff_out);
    	uart_tx.flush();
		
		try{
			long tout = System.currentTimeMillis() + tmr_out;					
			while(uart_rx.available() < buff_siz){
				Thread.yield();
				if (System.currentTimeMillis() > tout){
					break;
				}
			}
			uart_rx.read(buff_inp, 0, buff_siz); 
            for (byte x : buff_inp){
            	sb.append(String.format("%02X ", x));
            }
            if (Tx_chk() != Rx_chk()) {
            	sb.append("Error crc");
            } else if (buff_out[1] == (byte)0xE9){
        		cur_pos = (((int)buff_inp[5] * 256) + (int)buff_inp[6]) ;
        	}
		} catch(Exception e) {
			sb.append("Error input");
		}
		return sb.toString();    	
    }
    
	public String flush_rx() throws IOException{
		StringBuilder sb = new StringBuilder();		
		int inav = uart_rx.available();
		
		if(inav > 0){
			byte[] buff_tmp = new byte[inav];
			uart_rx.read(buff_tmp, 0, inav); 

            for (byte x : buff_tmp){
            	sb.append(String.format("%02X ", x));
            }
		} else {
			//sb.append("No input");			
		}
		
        return sb.toString();	
	}
	
	public int get_cur_pos(){
		return cur_pos;
	}
	
    public void set_uart(Uart urt){
    	uart_tx = urt.getOutputStream();
    	uart_rx = urt.getInputStream();
    }
    
    public void set_motor_id(int id){
    	motor_id = id;
    } 
    
    public void set_min_pos(int pos){
    	min_pos = pos;
    } 

    public void set_max_pos(int pos){
    	max_pos = pos;
    } 
    
    public String Motor_off() throws IOException{
    	buff_out[0] = (byte)0x80;
    	buff_out[1] = (byte)0xEB;
    	buff_out[2] = (byte)motor_id;
    	buff_out[3] = (byte)0x00;
    	
    	return Motor_send();
    	
    }
    
    public String Motor_on() throws IOException{
    	buff_out[0] = (byte)0x80;
    	buff_out[1] = (byte)0xEB;
    	buff_out[2] = (byte)motor_id;
    	buff_out[3] = (byte)0x01;
    	
    	return Motor_send();
    	
    }
    
    public String Motor_speed(int speed) throws IOException{
    	if (speed < 0){
    		speed = 0;
    	} else if(speed > 100) {
    		speed = 100;
    	}    	
    	
    	buff_out[0] = (byte)0x80; 
    	buff_out[1] = (byte)0xE9;
    	buff_out[2] = (byte)motor_id;
    	buff_out[3] = (byte)speed;
    	
    	return Motor_send();
	
    }
    
    public String Motor_pos(int pos) throws IOException{
    	if (pos < min_pos){
    		pos = min_pos;
    	} else if (pos > max_pos) {
    		pos = max_pos;
    	}    	
    	
		buff_out[0] = (byte)0x80;
		buff_out[1] = (byte)motor_id;
		byte hit = (byte)(int)(pos / 256);
		buff_out[2] = hit;
		byte low = (byte)(pos % 256);
		buff_out[3] = low;
    	
		cur_pos = pos;
		
    	return Motor_send();
			
    }
    
}

File Activity main.xml


<?xml version="1.0" encoding="utf-8"?>
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
    android:layout_width="fill_parent"
    android:layout_height="fill_parent"
    android:orientation="vertical" >

	<LinearLayout
	    android:layout_width="match_parent"
	    android:layout_height="wrap_content"
	    android:orientation="vertical"
	    android:paddingTop="20px" >

		<ToggleButton
		    android:id="@+id/toggleButton3"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:text="ToggleButton"
		    android:textOff="LED OFF"
		    android:textOn="LED ON" />

		<SeekBar
		    android:id="@+id/seekBar1"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:max="@integer/IDMOTORI"
		    android:paddingLeft="20dp"
		    android:paddingRight="20dp"
		    android:progress="0" />

		<SeekBar
		    android:id="@+id/seekBar2"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:layout_weight="1"
		    android:max="@integer/GRADIMOTORE"
		    android:paddingLeft="20dp"
		    android:paddingRight="20dp"
		    android:progress="1" />

		<ToggleButton
		    android:id="@+id/toggleButton1"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:text="ToggleButton"
		    android:textOff="MOTOR OFF"
		    android:textOn="MOTOR ON" />

		<SeekBar
		    android:id="@+id/seekBar3"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:layout_weight="1"
		    android:max="@integer/IDMOTORI"
		    android:paddingLeft="20dp"
		    android:paddingRight="20dp"
		    android:progress="0" />

		<SeekBar
		    android:id="@+id/seekBar4"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:layout_weight="1"
		    android:max="@integer/GRADIMOTORE"
		    android:paddingLeft="20dp"
		    android:paddingRight="20dp"
		    android:progress="1" />

		<ToggleButton
		    android:id="@+id/toggleButton2"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:text="ToggleButton"
		    android:textOff="MOTOR OFF"
		    android:textOn="MOTOR ON" />

		<SeekBar
		    android:id="@+id/seekBar5"
		    android:layout_width="match_parent"
		    android:layout_height="wrap_content"
		    android:max="100"
		    android:paddingLeft="20dp"
		    android:paddingRight="20dp"
		    android:progress="50" />
	
	</LinearLayout>

	<TextView
	    android:id="@+id/textView1"
	    android:layout_width="match_parent"
	    android:layout_height="match_parent"
	    android:textAppearance="?android:attr/textAppearanceSmall" />

</LinearLayout>

File RoboZakApp.java


package robozak.examples.simple;

import java.io.IOException;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.IOIO;
import ioio.lib.api.Uart;
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.SeekBar;
import android.widget.SeekBar.OnSeekBarChangeListener;
import android.widget.TextView;
import android.widget.ToggleButton;

public class RoboZakApp extends IOIOActivity {
	 TextView textView_;
	 SeekBar seekBar1;
	 SeekBar seekBar2;
	 SeekBar seekBar3;
	 SeekBar seekBar4;
	 SeekBar seekBar5;
	 ToggleButton toggleButton3;
	 ToggleButton toggleButton1;
	 ToggleButton toggleButton2;
	 int oldpos1;
	 int oldpos2;
	 int oldsped;
	 int oldmot1;
	 int oldmot2;
	 boolean oldmotoronoff1;
	 boolean oldmotoronoff2;
	 int cnrlog = 0;
	
    @Override
    public void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.main);

        textView_ = (TextView)findViewById(R.id.textView1);
        seekBar1 = (SeekBar)findViewById(R.id.seekBar1);
        seekBar2 = (SeekBar)findViewById(R.id.seekBar2);
        seekBar3 = (SeekBar)findViewById(R.id.seekBar3);
        seekBar4 = (SeekBar)findViewById(R.id.seekBar4);
        seekBar5 = (SeekBar)findViewById(R.id.seekBar5);
        toggleButton3 = (ToggleButton)findViewById(R.id.toggleButton3);
        toggleButton1 = (ToggleButton)findViewById(R.id.toggleButton1);
        toggleButton2 = (ToggleButton)findViewById(R.id.toggleButton2);
        
        seekBar1.setOnSeekBarChangeListener(new OnSeekBarChangeListener(){

			@Override
			public void onProgressChanged(SeekBar seekBar, int progress,
					boolean fromUser) {
				// TODO Auto-generated method stub
				setText("CNL 1 > MOT " + Integer.toString(seekBar1.getProgress() + 1));
			}

			@Override
			public void onStartTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}

			@Override
			public void onStopTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}   

        });   

        seekBar2.setOnSeekBarChangeListener(new OnSeekBarChangeListener(){

			@Override
			public void onProgressChanged(SeekBar seekBar, int progress,
					boolean fromUser) {
				// TODO Auto-generated method stub
				setText("CNL 1 > POS " + Integer.toString(seekBar2.getProgress() + 600));
			}

			@Override
			public void onStartTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}

			@Override
			public void onStopTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}   

        });   
        
        seekBar3.setOnSeekBarChangeListener(new OnSeekBarChangeListener(){

			@Override
			public void onProgressChanged(SeekBar seekBar, int progress,
					boolean fromUser) {
				// TODO Auto-generated method stub
				setText("CNL 2 > MOT " + Integer.toString(seekBar3.getProgress() + 1));
			}

			@Override
			public void onStartTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}

			@Override
			public void onStopTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}   

        });    

        seekBar4.setOnSeekBarChangeListener(new OnSeekBarChangeListener(){

			@Override
			public void onProgressChanged(SeekBar seekBar, int progress,
					boolean fromUser) {
				// TODO Auto-generated method stub
				setText("CNL 2 > POS " + Integer.toString(seekBar4.getProgress() + 600));
			}

			@Override
			public void onStartTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}

			@Override
			public void onStopTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}   

        });         
        
        seekBar5.setOnSeekBarChangeListener(new OnSeekBarChangeListener(){

			@Override
			public void onProgressChanged(SeekBar seekBar, int progress,
					boolean fromUser) {
				// TODO Auto-generated method stub
				setText("CNL x > VEL " + Integer.toString(seekBar5.getProgress() + 1));
			}

			@Override
			public void onStartTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}

			@Override
			public void onStopTrackingTouch(SeekBar seekBar) {
				// TODO Auto-generated method stub				
			}   

        }); 
        
        enableUi(false);
        
    }
	
	class Looper extends BaseIOIOLooper {
		 DigitalOutput led_;
		 Uart uart1;
		 Uart uart2;

		 Motore[] sin = new Motore[11];
		 Motore[] des = new Motore[11];
        
		@Override
		public void setup() throws ConnectionLostException, InterruptedException {
			try {
				oldpos1 = -1;
				oldpos2 = -1;
				oldsped = -1;
				oldmot1 = -1;
				oldmot2 = -1;
				
				led_ = ioio_.openDigitalOutput(IOIO.LED_PIN, true);
				uart1 = ioio_.openUart(5, 3, 19200, Uart.Parity.NONE, Uart.StopBits.TWO);
				uart2 = ioio_.openUart(6, 4, 19200, Uart.Parity.NONE, Uart.StopBits.TWO);

		        oldmotoronoff1 =! toggleButton1.isChecked();
		        oldmotoronoff2 =! toggleButton2.isChecked();
		        
		        for (int i=0;i<11;i++){
		        	StringBuilder sb = new StringBuilder();
		        	sin[i] = new Motore(uart1, i+1, seekBar5.getProgress(), sb);
		        	setText(sb.toString());
					
		        	sb = new StringBuilder();
		        	des[i] = new Motore(uart2, i+1, seekBar5.getProgress(), sb);
		        	setText(sb.toString());	        
		        }

				enableUi(true);
				
			} catch (ConnectionLostException e) {
				setText("Connection Lost Exception");
				enableUi(false);
				throw e;
			} catch (IOException e) {
				// TODO Blocco catch generato automaticamente
				e.printStackTrace();
			}
					
	}
		
		@Override
		public void loop() throws ConnectionLostException {
			try {

				led_.write(!toggleButton3.isChecked());

				if (toggleButton1.isChecked()!=oldmotoronoff1){
					oldmotoronoff1 = toggleButton1.isChecked();
					
					if (oldmotoronoff1 == true) {
						setText(sin[seekBar1.getProgress()].Motor_on());						
					} else {
						setText(sin[seekBar1.getProgress()].Motor_off());						
					}
				}

				if (toggleButton2.isChecked()!=oldmotoronoff2){
					oldmotoronoff2 = toggleButton2.isChecked();
					
					if (oldmotoronoff1 == true) { 
						setText(des[seekBar1.getProgress()].Motor_on());
					} else {
						setText(des[seekBar1.getProgress()].Motor_off());
					}
				}
				
				if (oldmot1 != seekBar1.getProgress()) {
					oldmot1 = seekBar1.getProgress();
					
					setText(sin[seekBar1.getProgress()].Motor_speed(seekBar5.getProgress()));
					setTextseekBar2(sin[seekBar1.getProgress()].get_cur_pos() - 600);
				}
				
				if (oldpos1 != seekBar2.getProgress()) {
					oldpos1 = seekBar2.getProgress();
					
					setText(sin[seekBar1.getProgress()].Motor_pos(seekBar2.getProgress() + 600));

				}
				
				if (oldmot2 != seekBar3.getProgress()) {
					oldmot2 = seekBar3.getProgress();
					
					setText(des[seekBar3.getProgress()].Motor_speed(seekBar5.getProgress()));
					setTextseekBar4(des[seekBar3.getProgress()].get_cur_pos() - 600);
				}
				
				if (oldpos2 != seekBar4.getProgress()) {
					oldpos2 = seekBar4.getProgress();
					
					setText(des[seekBar3.getProgress()].Motor_pos(seekBar4.getProgress() + 600));
			        
				}

				if (oldsped != seekBar5.getProgress()) {
					oldsped = seekBar5.getProgress();
					
					oldmot1 = -1;
					oldmot2 = -1;
					
				}				
				
				Thread.sleep(10);
			} catch (InterruptedException e) {
				setText("Interrupted Exception");
				ioio_.disconnect();
				enableUi(false);
			} catch (ConnectionLostException e) {
				setText("Connection Lost Exception");
				enableUi(false);
				throw e;
			} catch (IOException e) {
				setText("IO Exception");
				e.printStackTrace();
			}
			
		}
	}

	@Override
	protected IOIOLooper createIOIOLooper() {
		return new Looper();
	}

	private void enableUi(final boolean enable) {
		runOnUiThread(new Runnable() {
			@Override
			public void run() {
				seekBar1.setEnabled(enable);
				seekBar2.setEnabled(enable);
				seekBar3.setEnabled(enable);
				seekBar4.setEnabled(enable);
				seekBar5.setEnabled(enable);
				toggleButton3.setEnabled(enable);
				toggleButton1.setEnabled(enable);
				toggleButton2.setEnabled(enable);
				setText("Connesso " + Boolean.toString(enable));
			}
		});
	}
	
	private void setText(final String str) {
		runOnUiThread(new Runnable() {
			@Override
			public void run() {
				
				if (cnrlog < 50){
					cnrlog +=1;
					textView_.setText(str + "\n" + textView_.getText());
				} else {
					cnrlog =0;
					textView_.setText(str);
				}				
			}
		});
	}
	
	private void setTextseekBar2(final int prog) {
		runOnUiThread(new Runnable() {
			@Override
			public void run() {
				
				seekBar2.setProgress(prog);	
			}
		});
	}
	
	private void setTextseekBar4(final int prog) {
		runOnUiThread(new Runnable() {
			@Override
			public void run() {
				
				seekBar4.setProgress(prog);	
			}
		});
	}
	
}

File integers.xml


<?xml version="1.0" encoding="utf-8"?>
<resources>
    <integer name="GRADIMOTORE">3000</integer>
    <integer name="IDMOTORI">10</integer>
</resources>