git.haldean.org droidcopter / 4d0d9a9
Finished padcontroller bearing reading; Debugged direct control of motor speeds Benjamin Bardin 11 years ago
7 changed file(s) with 80 addition(s) and 37 deletion(s). Raw diff Collapse all Expand all
+1
-1
Pilot/.classpath less more
22 <classpathentry excluding="org/haldean/chopper/pilot/nav/NavListTest.java|org/haldean/chopper/pilot/nav/NavVelTest.java|org/haldean/chopper/server/|org/haldean/blob/JavaImage.java|org/haldean/blob/SegmentTest.java" including="org/haldean/blob/|org/haldean/chopper/nav/|org/haldean/chopper/pilot/" kind="src" path="src"/>
33 <classpathentry kind="src" path="gen"/>
44 <classpathentry kind="con" path="com.android.ide.eclipse.adt.ANDROID_FRAMEWORK"/>
5 <classpathentry kind="lib" path="/home/haldean/Git/droidcopter/jars/amarino.jar"/>
5 <classpathentry kind="lib" path="/home/benjamin/workspace/droidcopter/jars/amarino.jar"/>
66 <classpathentry kind="output" path="bin"/>
77 </classpath>
+2
-2
org/haldean/chopper/pilot/BluetoothOutput.java less more
2121
2222 public static void setMotorSpeeds(double m1, double m2, double m3, double m4) {
2323 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'A', (int) (100 * m1));
24 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'B', (int) (100 * m2));
25 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'C', (int) (100 * m3));
24 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'C', (int) (100 * m2));
25 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'B', (int) (100 * m3));
2626 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'D', (int) (100 * m4));
2727 }
2828 }⏎
+1
-0
org/haldean/chopper/pilot/ChopperMain.java less more
8686 comm.registerReceiver(NAV, nav);
8787 comm.registerReceiver(CSYS, nav);
8888 comm.registerReceiver(GUID, guid);
89 comm.registerReceiver(GUID, nav);
8990
9091 nav.registerReceiver(comm);
9192 nav.registerReceiver(guid);
+16
-8
org/haldean/chopper/pilot/Guidance.java less more
5757 /** Handles messages for the thread */
5858 private Handler mHandler;
5959
60 /** Stores whether or not a motor-eval loop should add itself to the queue again. **/
61 private boolean mReviseMotorSpeeds = true;
62
6063 /** Stores orientation data persistently, as expected values in case lock is not immediately available*/
6164 private double mAzimuth;
6265 private double mPitchDeg;
132135 //Temporary: need real tuning values at some point. Crap.
133136 for (int i = 0; i < 3; i++)
134137 for (int j = 0; j < 3; j++)
135 mGain[i][j] = .05;
138 mGain[i][j] = .005;
136139 for (int j = 0; j < 3; j++) {
137140 mGain[3][j] = .0005;
138141 }
156159 public void handleMessage(Message msg) {
157160 switch (msg.what) {
158161 case EVAL_MOTOR_SPEED:
162 Log.d(TAG, "evaluating motor speed");
159163 reviseMotorSpeed();
160164 //Log.d(TAG, getErrorString());
161165 updateReceivers(getErrorString());
187191 }
188192 }
189193 };
190 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
194 //mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
195 receiveMessage("GUID:VECTOR:0:0:0:0", null);
191196 Looper.loop();
192197 }
193198
197202 * @param source The source of the message, if a reply is needed. May be null.
198203 */
199204 public void receiveMessage(String msg, Receivable source) {
200 Log.d(TAG, "Receiving message " + msg);
205 //Log.d(TAG, "Receiving message " + msg);
201206 String[] parts = msg.split(":");
202207 if (parts[0].equals("GUID")) {
203208 if (parts[1].equals("PID")) {
216221 }
217222 if (parts[1].equals("AUTOMATIC")) {
218223 mHandler.removeMessages(NEW_GUID_VECTOR);
224 mReviseMotorSpeeds = true;
219225 if (!mHandler.hasMessages(EVAL_MOTOR_SPEED))
220226 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
221227 }
228234 mAbsVec = true;
229235 }
230236 if (parts[1].equals("VECTOR")) {
237 mReviseMotorSpeeds = false;
231238 mHandler.removeMessages(EVAL_MOTOR_SPEED);
232239 Double[] myVector = new Double[4];
233240 for (int i = 0; i < 4; i++) {
413420 case 3: //Azimuth
414421 break;
415422 }
416
417423 mTorques[i] = dmotor;
418424 //Log.v(TAG, "Torque " + i + " " + dmotor);
419425 }
471477 //Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]);
472478 //Sleep a while
473479 long timetonext = (1000 / PIDREPS) - (System.currentTimeMillis() - starttime);
474 if (timetonext > 0)
475 mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext);
476 else
477 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
480 if (mReviseMotorSpeeds) {
481 if (timetonext > 0)
482 mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext);
483 else
484 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
485 }
478486 }
479487
480488 /* To be finished */
+9
-3
org/haldean/chopper/pilot/Navigation.java less more
178178 setTask(BASIC_AUTO, taskList);
179179 setTask(NO_CONN, "{ VEL!No_Conn!0!0!-1!0!1000000!-1!-5 -6}");
180180 setTask(LOW_POWER, "{ VEL!Low_Power!0!0!-1!0!1000000!-1!-7 -8}");
181
182 mNavStatus.set(BASIC_AUTO);
183 autoPilot(true);
181 //mNavStatus.set(BASIC_AUTO);
182
183
184 //autoPilot(true);
184185
185186 Looper.loop();
186187 }
261262 public void receiveMessage(String msg, Receivable source) {
262263 Log.d(TAG, "Receiving " + msg);
263264 String[] parts = msg.split(":");
265 if (parts[0].equals("GUID")) {
266 if (parts[1].equals("VECTOR")) {
267 autoPilot(false);
268 }
269 }
264270 if (parts[0].equals("NAV")) {
265271 if (parts[1].equals("SET")) {
266272 Log.v(TAG, "Updating Nav Status");
+1
-1
org/haldean/chopper/server/EnsignCrusher.java less more
102102 for (int i=0; i<4; i++) {
103103 taskString += ":" + speeds[i];
104104 }
105 DataReceiver.sendToDefault(navGoToManual);
105 //DataReceiver.sendToDefault(navGoToManual);
106106 DataReceiver.sendToDefault(taskString);
107107 }
108108
+50
-22
org/haldean/chopper/server/PadController.java less more
4040
4141 private int lastButtonMask = 0;
4242
43 private boolean globeMovement = false;
43 private boolean globeMovement = true;
4444
4545 private final double minDiff = 250; //ms
4646 private long lastAxesUpdate;
47
48 private double theta = 0; //bearing
4749
4850 /** Create a new PadController
4951 * @param _ui The ServerHost to act upon */
155157 public boolean buttonIsSet(int button) {
156158 return buttonIsSet(lastButtonMask, button);
157159 }
160
161 /** Perform an action based on the status of the buttons
162 *
163 */
164 private void buttonMaybeAction() {
165 int currentMask = buttonMask();
166 /* In Conrol Mode, B changes orientation */
167 if (!globeMovement && buttonIsSet(currentMask, BUTTON_B)) {
168 double mX = getAxis(AXIS_L_H);
169 double mY = -getAxis(AXIS_L_V);
170 if ( Math.sqrt(mX * mX + mY * mY) > .7 ) { //only take measurements .7 radius out. Arbitrary. Magic Number. Deal.
171 double angle = Math.atan2(mY, mX);
172 angle = angle * 180.0 / Math.PI;
173 angle = 90.0 - angle;
174 if (angle < 0.0)
175 angle += 360.0;
176 theta = angle;
177 }
178 }
179 }
158180
159181 /** Perform an action based on the status of the buttons
160182 * @param mask The mask representings buttons that have changed state */
196218 sl.setGlobeMode(globeMovement);
197219 Debug.log("Globe control is now " + ((globeMovement) ? "on" : "off"));
198220 }
199
221
200222 /* In Globe Mode, B activates or disactivates follow mode */
201223 if (globeMovement && buttonIsSet(mask, BUTTON_B))
202224 ui.globeComponent.toggleFollow();
203225 }
204
226
227
205228 /** Get the value of a joystick axis, filtering out noisy results
206229 * @param axis The index of the axis to check
207230 * @return A number from -1 to 1 representing the value of the joystick */
218241 }
219242
220243 /** Trigger events based on the values of the axes */
221 private void axesAction() {
244 private void axesAction(int mask) {
222245 if (globeMovement) {
223246 float zoom = getAxis(AXIS_L_TRIGGER) - getAxis(AXIS_R_TRIGGER);
224247 ui.globeComponent.moveView(getAxis(AXIS_L_H), getAxis(AXIS_L_V),
225248 zoom, getAxis(AXIS_R_V), getAxis(AXIS_R_H));
226249 } else {
227250 double[] vels = new double[3];
228 vels[0] = getAxis(AXIS_L_H);
229 vels[1] = -getAxis(AXIS_L_V);
230 vels[2] = getAxis(AXIS_R_V);
251
252 if (!buttonIsSet(mask, BUTTON_B)) { //if it's set, we're taking bearing. don't alter velocity.
253 vels[0] = getAxis(AXIS_L_H);
254 vels[1] = -getAxis(AXIS_L_V);
255 vels[2] = getAxis(AXIS_R_V);
256
231257
232 boolean updateVec = false;
233 if (System.currentTimeMillis() - lastAxesUpdate > minDiff) {
234 updateVec = true;
235 }
236
237 if (updateVec) {
238 lastAxesUpdate = System.currentTimeMillis();
239 //3.0 is the value of the maximum normal vector
240 double adjustment = EnsignCrusher.MAX_VELOCITY / Math.sqrt(3.0);
241 for (double v : vels) {
242 v *= adjustment;
243 }
244
245 EnsignCrusher.manualVelocity(vels);
258 boolean updateVec = false;
259 if (System.currentTimeMillis() - lastAxesUpdate > minDiff) {
260 updateVec = true;
261 }
262
263 if (updateVec) {
264 lastAxesUpdate = System.currentTimeMillis();
265 //3.0 is the value of the maximum normal vector
266 double adjustment = EnsignCrusher.MAX_VELOCITY / Math.sqrt(3.0);
267 for (double v : vels) {
268 v *= adjustment;
269 }
270
271 EnsignCrusher.manualVelocity(vels, theta);
272 }
246273 }
247274 }
248275 }
258285 lastButtonMask = mask;
259286
260287 if (enabled) {
261 axesAction();
288 axesAction(mask);
262289 if (newButtons != 0) {
263290 buttonAction(newButtons);
264291 }
292 buttonMaybeAction();
265293 }
266294
267295 try {