Figures for paper; Some guidance PID updates; small bugfix
Benjamin Bardin
11 years ago
335 | 335 | ConnectedThread ct = connections.get(address); |
336 | 336 | if (ct != null) { |
337 | 337 | ct.write(data); |
338 | /*try { | |
338 | try { | |
339 | 339 | if (logfile != null) |
340 | 340 | logfile.write(Long.toString(System.currentTimeMillis()) + " " + new String(data) + "\n"); |
341 | 341 | } |
342 | 342 | catch (IOException e) { |
343 | 343 | Log.e(TAG, "Cannot write to logfile."); |
344 | 344 | e.printStackTrace(); |
345 | }*/ | |
345 | } | |
346 | 346 | } |
347 | 347 | } |
348 | 348 |
0 | 0 | package org.haldean.chopper.pilot; |
1 | 1 | |
2 | import android.content.Context; | |
3 | 2 | import android.content.Intent; |
4 | import android.content.ServiceConnection; | |
5 | 3 | import android.os.Handler; |
6 | 4 | import android.os.Looper; |
7 | 5 | import android.os.Message; |
8 | import android.util.Log; | |
9 | 6 | import at.abraxas.amarino.Amarino; |
10 | 7 | import at.abraxas.amarino.AmarinoIntent; |
11 | 8 | import at.abraxas.amarino.AmarinoService; |
12 | 9 | |
10 | /** | |
11 | * Interfaces Pilot with Amarino | |
12 | */ | |
13 | 13 | public class BluetoothOutput implements Constants, Runnable { |
14 | private ChopperMain context; | |
15 | private volatile boolean initialized; | |
16 | 14 | static final String BT_DEVICE_ADDR = "00:06:66:04:B1:BE"; |
17 | 15 | public Handler mHandler; |
18 | 16 | private AmarinoService amService; |
19 | private ServiceConnection mConnection; | |
20 | 17 | public static final String TAG = "BluetoothOutput"; |
21 | 18 | |
22 | public BluetoothOutput(ChopperMain context) { | |
23 | this.context = context; | |
24 | initialized = true; | |
25 | } | |
26 | ||
19 | /** | |
20 | * Initialize, start the message handler. | |
21 | */ | |
27 | 22 | public void run() { |
28 | 23 | Looper.prepare(); |
29 | 24 | Thread.currentThread().setName("BluetoothOutput"); |
43 | 38 | Looper.loop(); |
44 | 39 | } |
45 | 40 | |
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)); | |
55 | 53 | } |
56 | 54 | |
57 | 55 | /** |
62 | 60 | * @param flag the flag Arduino has registered a function for to receive this data |
63 | 61 | * @param data your data you want to send |
64 | 62 | */ |
65 | private void sendDataToArduino(Context context, String address, char flag, int data) { | |
63 | private void sendDataToArduino(String address, char flag, int data) { | |
66 | 64 | Intent intent = getSendIntent(address, AmarinoIntent.INT_EXTRA, flag); |
67 | 65 | intent.putExtra(AmarinoIntent.EXTRA_DATA, data); |
68 | 66 | amService = Amarino.getAmarinoService(); |
73 | 71 | //Log.e(TAG, "amarino service is null"); |
74 | 72 | } |
75 | 73 | |
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 | */ | |
76 | 81 | private static Intent getSendIntent(String address, int dataType, char flag){ |
77 | 82 | Intent intent = new Intent(AmarinoIntent.ACTION_SEND); |
78 | 83 | intent.putExtra(AmarinoIntent.EXTRA_DEVICE_ADDRESS, address); |
14 | 14 | { |
15 | 15 | /** Tag for logging */ |
16 | 16 | public static final String TAG = "chopper.ChopperMain"; |
17 | private boolean telemetry = true; | |
18 | 17 | |
18 | private boolean telemetry = false; | |
19 | ||
20 | /** Prevent duplicate launching of threads if app loses focus **/ | |
19 | 21 | private static boolean mFirstRun = true; |
22 | ||
23 | /** Need a permanent reference to this, to destroy it. **/ | |
20 | 24 | private Guidance guid; |
21 | 25 | |
22 | 26 | /** |
32 | 36 | super(); |
33 | 37 | } |
34 | 38 | |
35 | @Override | |
39 | /** | |
40 | * Starts the activity, and Amarino | |
41 | */ | |
36 | 42 | public void onStart() { |
37 | 43 | super.onStart(); |
38 | 44 | Amarino.connect(this, BluetoothOutput.BT_DEVICE_ADDR); |
39 | 45 | } |
40 | 46 | |
41 | @Override | |
47 | /** | |
48 | * Stops the activity, and Amarino | |
49 | */ | |
42 | 50 | public void onStop() { |
43 | 51 | super.onStop(); |
44 | 52 | Amarino.disconnect(this, BluetoothOutput.BT_DEVICE_ADDR); |
76 | 84 | if (telemetry) { |
77 | 85 | pic = new MakePicture(previewHolder); |
78 | 86 | } |
79 | BluetoothOutput mBTooth = new BluetoothOutput(this); | |
87 | BluetoothOutput mBTooth = new BluetoothOutput(); | |
80 | 88 | Navigation nav = new Navigation(status); |
81 | 89 | guid = new Guidance(status, nav, mBTooth); |
82 | 90 | |
124 | 132 | protected void onDestroy() { |
125 | 133 | //mWakeLock.release(); |
126 | 134 | super.onDestroy(); |
127 | guid.onDestroy(); | |
135 | if (guid != null) | |
136 | guid.onDestroy(); | |
128 | 137 | } |
129 | 138 | } |
70 | 70 | private double mGpsSpeed; |
71 | 71 | private double mGpsDalt; |
72 | 72 | |
73 | /** Log file name **/ | |
73 | 74 | public static final String logname = "/sdcard/chopper/guidlog.txt"; |
75 | ||
76 | /** Log file writer **/ | |
74 | 77 | private FileWriter logfile; |
75 | 78 | |
76 | 79 | /** Note that some of the following objects are declared outside their smallest scope. |
115 | 118 | * in the event of difficulty maintaining altitude */ |
116 | 119 | private boolean mHorizontalDrift = false; //if true, does not consider dx, dy or azimuth error; makes for maximally efficient altitude control |
117 | 120 | |
121 | /** List of registered receivers */ | |
118 | 122 | private LinkedList<Receivable> mRec; |
119 | 123 | |
120 | 124 | /** Handles to other chopper components */ |
121 | 125 | private ChopperStatus mStatus; |
122 | 126 | private Navigation mNav; |
127 | private BluetoothOutput mBt; | |
123 | 128 | |
124 | 129 | /** Controls whether N/S and E/W commands refer to absolute vectors or local **/ |
125 | 130 | private boolean mAbsVec = true; |
126 | 131 | |
127 | private BluetoothOutput mBt; | |
128 | ||
132 | /** Flag for writing motor speeds to output file **/ | |
129 | 133 | public final static boolean mEnableLogging = true; |
134 | ||
130 | 135 | /** |
131 | 136 | * Constructs a Guidance object |
132 | 137 | * @param status The source status information. |
146 | 151 | for (int j = 0; j < 3; j++) |
147 | 152 | mGain[i][j] = .0003; |
148 | 153 | //mGain[i][j] = .05; |
154 | ||
149 | 155 | for (int j = 0; j < 3; j++) { |
150 | 156 | // mGain[3][j] = .0005; |
151 | mGain[2][j] = .0015; | |
157 | mGain[2][j] = .0025; | |
152 | 158 | mGain[3][j] = 0; |
153 | 159 | } |
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 | ||
154 | 171 | try { |
155 | 172 | if (mEnableLogging) |
156 | 173 | logfile = new FileWriter(logname, false); |
161 | 178 | } |
162 | 179 | } |
163 | 180 | |
181 | /** | |
182 | * Obtains the current P error values, concatenates into a string | |
183 | * @return A string representing the error values. | |
184 | */ | |
164 | 185 | private String getErrorString() { |
165 | 186 | return "GUID:ERROR:" + mErrors[0][0] |
166 | 187 | + ":" + mErrors[1][0] |
168 | 189 | + ":" + mErrors[3][0]; |
169 | 190 | } |
170 | 191 | |
192 | /** | |
193 | * Closes the log file. | |
194 | */ | |
171 | 195 | public void onDestroy() { |
172 | 196 | try { |
173 | 197 | if (logfile != null) |
522 | 546 | else if (mTempMotorSpeed[i] > 1) |
523 | 547 | mTempMotorSpeed[i] = 1; |
524 | 548 | 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); | |
527 | 551 | if (mStabilizing) { |
528 | 552 | if (diff > 0) |
529 | 553 | mMotorSpeed[i] += Math.min(diff, MAX_DSTABLE); |
532 | 556 | } |
533 | 557 | else { |
534 | 558 | if (diff > 0) { |
535 | if (i==1) | |
536 | Log.v(TAG, "guid, adding " + Math.min(diff, MAX_DMOTOR)); | |
537 | 559 | mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR); |
538 | 560 | } |
539 | 561 | else if (diff < 0) { |
540 | if (i==1) | |
541 | Log.v(TAG, "guid, adding " + Math.max(diff, -MAX_DMOTOR)); | |
542 | 562 | mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR); |
543 | 563 | } |
544 | 564 | } |
545 | 565 | 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]); | |
548 | 568 | } |
549 | 569 | |
550 | 570 | //Send motor values to motors here: |
563 | 583 | } |
564 | 584 | } |
565 | 585 | |
566 | /* To be finished */ | |
586 | /** | |
587 | * Write motor values to ChopperStatus, BluetoothOutput, logfile. | |
588 | */ | |
567 | 589 | private void updateMotors() { |
568 | 590 | //Pass filtered values to ChopperStatus. |
569 | 591 | mStatus.setMotorFields(mMotorSpeed); |
136 | 136 | } |
137 | 137 | return true; |
138 | 138 | } |
139 | ||
139 | ||
140 | /** | |
141 | * Get velocity target from a NavTrack | |
142 | * @param nav The NavTrack | |
143 | * @param target Writes the velocity here. | |
144 | */ | |
140 | 145 | private void getTrackTarg(NavTrack nav, double[] target) { |
141 | 146 | if (!nav.started()) { |
142 | 147 | nav.start(); |
143 | 148 | } |
144 | 149 | } |
145 | 150 | |
146 | ||
151 | /** | |
152 | * Get velocity target from a NavVel | |
153 | * @param nav The NavVel | |
154 | * @param target Writes the velocity here. | |
155 | */ | |
147 | 156 | private void getVelTarg(NavData nav, double[] target) { |
148 | 157 | NavVel vel = (NavVel) nav; |
149 | 158 | |
157 | 166 | } |
158 | 167 | } |
159 | 168 | |
169 | /** | |
170 | * Get velocity target from a NavDest | |
171 | * @param nav The NavDest | |
172 | * @param target Writes the velocity here. | |
173 | */ | |
160 | 174 | private void getDestTarg(NavData nav, double[] target) { |
161 | 175 | NavDest lastDest = (NavDest) nav; |
162 | 176 | Location myLoc = myCs.getLastLocation(); |
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown