git.haldean.org droidcopter / db59e46
Figures for paper; Some guidance PID updates; small bugfix Benjamin Bardin 11 years ago
9 changed file(s) with 92 addition(s) and 42 deletion(s). Raw diff Collapse all Expand all
335335 ConnectedThread ct = connections.get(address);
336336 if (ct != null) {
337337 ct.write(data);
338 /*try {
338 try {
339339 if (logfile != null)
340340 logfile.write(Long.toString(System.currentTimeMillis()) + " " + new String(data) + "\n");
341341 }
342342 catch (IOException e) {
343343 Log.e(TAG, "Cannot write to logfile.");
344344 e.printStackTrace();
345 }*/
345 }
346346 }
347347 }
348348
00 package org.haldean.chopper.pilot;
11
2 import android.content.Context;
32 import android.content.Intent;
4 import android.content.ServiceConnection;
53 import android.os.Handler;
64 import android.os.Looper;
75 import android.os.Message;
8 import android.util.Log;
96 import at.abraxas.amarino.Amarino;
107 import at.abraxas.amarino.AmarinoIntent;
118 import at.abraxas.amarino.AmarinoService;
129
10 /**
11 * Interfaces Pilot with Amarino
12 */
1313 public class BluetoothOutput implements Constants, Runnable {
14 private ChopperMain context;
15 private volatile boolean initialized;
1614 static final String BT_DEVICE_ADDR = "00:06:66:04:B1:BE";
1715 public Handler mHandler;
1816 private AmarinoService amService;
19 private ServiceConnection mConnection;
2017 public static final String TAG = "BluetoothOutput";
2118
22 public BluetoothOutput(ChopperMain context) {
23 this.context = context;
24 initialized = true;
25 }
26
19 /**
20 * Initialize, start the message handler.
21 */
2722 public void run() {
2823 Looper.prepare();
2924 Thread.currentThread().setName("BluetoothOutput");
4338 Looper.loop();
4439 }
4540
46 public boolean initialized() {
47 return initialized;
48 }
49
50 public void setMotorSpeeds(double m1, double m2, double m3, double m4) {
51 sendDataToArduino(context, BT_DEVICE_ADDR, 'A', (int) (100 * m1));
52 sendDataToArduino(context, BT_DEVICE_ADDR, 'C', (int) (100 * m2));
53 sendDataToArduino(context, BT_DEVICE_ADDR, 'B', (int) (100 * m3));
54 sendDataToArduino(context, BT_DEVICE_ADDR, 'D', (int) (100 * m4));
41 /**
42 * Send motor speeds to arduino.
43 * @param m1 First motor's speed.
44 * @param m2 Second motor's speed.
45 * @param m3 Third motor's speed.
46 * @param m4 Fourth motor's speed.
47 */
48 private void setMotorSpeeds(double m1, double m2, double m3, double m4) {
49 sendDataToArduino(BT_DEVICE_ADDR, 'A', (int) (100 * m1));
50 sendDataToArduino(BT_DEVICE_ADDR, 'C', (int) (100 * m2));
51 sendDataToArduino(BT_DEVICE_ADDR, 'B', (int) (100 * m3));
52 sendDataToArduino(BT_DEVICE_ADDR, 'D', (int) (100 * m4));
5553 }
5654
5755 /**
6260 * @param flag the flag Arduino has registered a function for to receive this data
6361 * @param data your data you want to send
6462 */
65 private void sendDataToArduino(Context context, String address, char flag, int data) {
63 private void sendDataToArduino(String address, char flag, int data) {
6664 Intent intent = getSendIntent(address, AmarinoIntent.INT_EXTRA, flag);
6765 intent.putExtra(AmarinoIntent.EXTRA_DATA, data);
6866 amService = Amarino.getAmarinoService();
7371 //Log.e(TAG, "amarino service is null");
7472 }
7573
74 /**
75 * Form Intent to pass to AmarinoService
76 * @param address Address of the arduino
77 * @param dataType Add this as an "extra."
78 * @param flag Which motor to change.
79 * @return
80 */
7681 private static Intent getSendIntent(String address, int dataType, char flag){
7782 Intent intent = new Intent(AmarinoIntent.ACTION_SEND);
7883 intent.putExtra(AmarinoIntent.EXTRA_DEVICE_ADDRESS, address);
1414 {
1515 /** Tag for logging */
1616 public static final String TAG = "chopper.ChopperMain";
17 private boolean telemetry = true;
1817
18 private boolean telemetry = false;
19
20 /** Prevent duplicate launching of threads if app loses focus **/
1921 private static boolean mFirstRun = true;
22
23 /** Need a permanent reference to this, to destroy it. **/
2024 private Guidance guid;
2125
2226 /**
3236 super();
3337 }
3438
35 @Override
39 /**
40 * Starts the activity, and Amarino
41 */
3642 public void onStart() {
3743 super.onStart();
3844 Amarino.connect(this, BluetoothOutput.BT_DEVICE_ADDR);
3945 }
4046
41 @Override
47 /**
48 * Stops the activity, and Amarino
49 */
4250 public void onStop() {
4351 super.onStop();
4452 Amarino.disconnect(this, BluetoothOutput.BT_DEVICE_ADDR);
7684 if (telemetry) {
7785 pic = new MakePicture(previewHolder);
7886 }
79 BluetoothOutput mBTooth = new BluetoothOutput(this);
87 BluetoothOutput mBTooth = new BluetoothOutput();
8088 Navigation nav = new Navigation(status);
8189 guid = new Guidance(status, nav, mBTooth);
8290
124132 protected void onDestroy() {
125133 //mWakeLock.release();
126134 super.onDestroy();
127 guid.onDestroy();
135 if (guid != null)
136 guid.onDestroy();
128137 }
129138 }
7070 private double mGpsSpeed;
7171 private double mGpsDalt;
7272
73 /** Log file name **/
7374 public static final String logname = "/sdcard/chopper/guidlog.txt";
75
76 /** Log file writer **/
7477 private FileWriter logfile;
7578
7679 /** Note that some of the following objects are declared outside their smallest scope.
115118 * in the event of difficulty maintaining altitude */
116119 private boolean mHorizontalDrift = false; //if true, does not consider dx, dy or azimuth error; makes for maximally efficient altitude control
117120
121 /** List of registered receivers */
118122 private LinkedList<Receivable> mRec;
119123
120124 /** Handles to other chopper components */
121125 private ChopperStatus mStatus;
122126 private Navigation mNav;
127 private BluetoothOutput mBt;
123128
124129 /** Controls whether N/S and E/W commands refer to absolute vectors or local **/
125130 private boolean mAbsVec = true;
126131
127 private BluetoothOutput mBt;
128
132 /** Flag for writing motor speeds to output file **/
129133 public final static boolean mEnableLogging = true;
134
130135 /**
131136 * Constructs a Guidance object
132137 * @param status The source status information.
146151 for (int j = 0; j < 3; j++)
147152 mGain[i][j] = .0003;
148153 //mGain[i][j] = .05;
154
149155 for (int j = 0; j < 3; j++) {
150156 // mGain[3][j] = .0005;
151 mGain[2][j] = .0015;
157 mGain[2][j] = .0025;
152158 mGain[3][j] = 0;
153159 }
160
161 mGain[0][0] = .001;
162 mGain[0][1] = .0003;
163 mGain[0][2] = 0;
164 mGain[1][0] = .001;
165 mGain[1][1] = .0003;
166 mGain[1][2] = 0;
167 mGain[2][0] = .006;
168 mGain[2][1] = .006;
169 mGain[2][2] = 0;
170
154171 try {
155172 if (mEnableLogging)
156173 logfile = new FileWriter(logname, false);
161178 }
162179 }
163180
181 /**
182 * Obtains the current P error values, concatenates into a string
183 * @return A string representing the error values.
184 */
164185 private String getErrorString() {
165186 return "GUID:ERROR:" + mErrors[0][0]
166187 + ":" + mErrors[1][0]
168189 + ":" + mErrors[3][0];
169190 }
170191
192 /**
193 * Closes the log file.
194 */
171195 public void onDestroy() {
172196 try {
173197 if (logfile != null)
522546 else if (mTempMotorSpeed[i] > 1)
523547 mTempMotorSpeed[i] = 1;
524548 double diff = mTempMotorSpeed[i] - mMotorSpeed[i];
525 if (i==1)
526 Log.v(TAG, "guid, diff is " + diff);
549 //if (i==1)
550 //Log.v(TAG, "guid, diff is " + diff);
527551 if (mStabilizing) {
528552 if (diff > 0)
529553 mMotorSpeed[i] += Math.min(diff, MAX_DSTABLE);
532556 }
533557 else {
534558 if (diff > 0) {
535 if (i==1)
536 Log.v(TAG, "guid, adding " + Math.min(diff, MAX_DMOTOR));
537559 mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR);
538560 }
539561 else if (diff < 0) {
540 if (i==1)
541 Log.v(TAG, "guid, adding " + Math.max(diff, -MAX_DMOTOR));
542562 mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
543563 }
544564 }
545565 mTempMotorSpeed[i] = mMotorSpeed[i];
546 if (i == 1)
547 Log.v(TAG, "guid, ms1 is " + mMotorSpeed[i]);
566 //if (i == 1)
567 // Log.v(TAG, "guid, ms1 is " + mMotorSpeed[i]);
548568 }
549569
550570 //Send motor values to motors here:
563583 }
564584 }
565585
566 /* To be finished */
586 /**
587 * Write motor values to ChopperStatus, BluetoothOutput, logfile.
588 */
567589 private void updateMotors() {
568590 //Pass filtered values to ChopperStatus.
569591 mStatus.setMotorFields(mMotorSpeed);
136136 }
137137 return true;
138138 }
139
139
140 /**
141 * Get velocity target from a NavTrack
142 * @param nav The NavTrack
143 * @param target Writes the velocity here.
144 */
140145 private void getTrackTarg(NavTrack nav, double[] target) {
141146 if (!nav.started()) {
142147 nav.start();
143148 }
144149 }
145150
146
151 /**
152 * Get velocity target from a NavVel
153 * @param nav The NavVel
154 * @param target Writes the velocity here.
155 */
147156 private void getVelTarg(NavData nav, double[] target) {
148157 NavVel vel = (NavVel) nav;
149158
157166 }
158167 }
159168
169 /**
170 * Get velocity target from a NavDest
171 * @param nav The NavDest
172 * @param target Writes the velocity here.
173 */
160174 private void getDestTarg(NavData nav, double[] target) {
161175 NavDest lastDest = (NavDest) nav;
162176 Location myLoc = myCs.getLastLocation();