git.haldean.org droidcopter / 4f22e70
Updated everything for flight Will Brown 10 years ago
10 changed file(s) with 115 addition(s) and 92 deletion(s). Raw diff Collapse all Expand all
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"/>
56 <classpathentry kind="output" path="bin"/>
67 </classpath>
1313 #define WORD_LENGTH 2
1414
1515 #include <Servo.h>
16 #include <MeetAndroid.h>
17
18 unsigned int led_cycles = 0;
19 MeetAndroid bt;
1620
1721 struct motor {
1822 unsigned char pin;
2529 {6, 0, 0, Servo()}, {9, 0, 0, Servo()},
2630 {10, 0, 0, Servo()}, {11, 0, 0, Servo()}
2731 };
28
29 char buffer[WORD_BUFFER_LENGTH];
30 unsigned int led_cycles = 0;
3132
3233 /**
3334 * Updates the speeds of the motors.
4344 * Print the speeds of the motors to serial.
4445 */
4546 void print_speeds(void) {
46 Serial.print(SPEED_HEADER);
4747 for (int i = 0; i < MOTOR_COUNT; i++) {
48 Serial.print(":");
49 Serial.print(motors[i].current_speed);
48 bt.send(motors[i].current_speed);
5049 }
51 Serial.println();
5250 }
5351
5452 /**
5755 void init_motors(void) {
5856 for (int i = 0; i < MOTOR_COUNT; i++) {
5957 motors[i].ctrl.attach(motors[i].pin);
58 motors[i].next_speed = MOTOR_OFF_COMMAND;
6059 }
6160
6261 long time = millis();
6564 motors[i].ctrl.write(MOTOR_ARM_VALUE);
6665 }
6766 }
67
68 write_speeds();
6869 }
6970
7071 /**
7172 * Initialize messenger service and speed vector.
7273 */
7374 void setup(void) {
74 /* Initialize serial monitor. */
75 Serial.begin(115200);
76
7775 /* Set the serial status LED to output. */
7876 pinMode(STATUS_LED, OUTPUT);
7977
8078 /* Initialize the motor controllers. */
8179 init_motors();
8280
83 Serial.println("BEGIN");
81 Serial.begin(115200);
82 char flag;
83 for (flag='A'; flag<='D'; flag++) {
84 bt.registerFunction(motor_message, flag);
85 }
86 }
87
88 /**
89 * Read a message from Android.
90 */
91 void motor_message(byte flag, byte numOfValues) {
92 int i, motor_val;
93
94 if (flag == 'A') i = 0;
95 else if (flag == 'B') i = 1;
96 else if (flag == 'C') i = 2;
97 else if (flag == 'D') i = 3;
98
99 motor_val = bt.getInt();
100
101 if (motor_val) {
102 motors[i].next_speed = map(motor_val, -1, 99, MOTOR_MIN_COMMAND, MOTOR_MAX_COMMAND);
103 } else {
104 motors[i].next_speed = MOTOR_OFF_COMMAND;
105 }
106
107 write_speeds();
108 print_speeds();
109 led_cycles = 0;
84110 }
85111
86112 /**
87113 * Wait for serial information to come in.
88114 */
89115 void loop(void) {
90 if (Serial.available() >= WORD_LENGTH) {
91 int i, j, timeout, buffer_val;
92 led_cycles = 0;
93 digitalWrite(STATUS_LED, HIGH);
94
95 for (i=0; i<MOTOR_COUNT; i++) {
96 for (timeout=0; timeout < MESSAGE_TIMEOUT_CYCLES; timeout++) {
97 if (Serial.available() >= WORD_LENGTH) break;
98 }
99
100 if (Serial.available() >= WORD_LENGTH) {
101 for (j=0; j<WORD_LENGTH; j++) {
102 buffer[j] = Serial.read();
103 }
104
105 buffer[WORD_LENGTH] = '\0';
106 buffer_val = atoi(buffer);
107 if (buffer_val > 0) {
108 motors[i].next_speed = map(atoi(buffer), -1, 99,
109 MOTOR_MIN_COMMAND, MOTOR_MAX_COMMAND);
110 } else {
111 motors[i].next_speed = MOTOR_OFF_COMMAND;
112 }
113 } else {
114 Serial.println(BAD_REQUEST);
115 }
116 }
117
118 /* Flush serial buffer after reading a line. */
119 while (Serial.available()) {
120 Serial.print("");
121 Serial.read();
122 }
123
124 write_speeds();
125 print_speeds();
126 }
116 bt.receive();
127117
128118 led_cycles++;
129119 if (led_cycles >= STATUS_LED_CYCLES) {
132122
133123 #ifdef ENABLE_HEARTBEAT
134124 if (led_cycles % 10000 == 0) {
135 Serial.println(HEARTBEAT_PULSE);
125 bt.send(HEARTBEAT_PULSE);
136126 }
137127 #endif
138
139128 }
140129
3232 rgb >> 8 & 0xFF,
3333 rgb & 0xFF};
3434 }
35
36 public void updateImage(byte[] data, int width, int height) {
37 /* Unimplemented. */
38 }
3539 }
2929 int targetArea = new Integer(parts[4]);
3030 int threshold = new Integer(parts[5]);
3131 return new Segmenter(targetColor, targetArea, threshold);
32 }
32 }
3333
3434 public static Segmenter getSegmenterForPoint(Image input, int x, int y) {
3535 Segmenter seg = new Segmenter(input.getPixel(x, y), 0, DEFAULT_THRESHOLD);
1414
1515 private static final int TRACKING_PERIOD_MS = 200;
1616 private static final int DISABLED_PERIOD_MS = 1000;
17 private static final int ASCEND_VELOCITY = 2;
1718
1819 public BlobTracker(MakePicture pic) {
1920 lastLocation = new int[2];
2627
2728 public void receiveMessage(String msg, Receivable source) {
2829 if (msg.startsWith("SEGMENT")) {
29 segmenter = Segmenter.fromString(msg);
30 segmenter = Segmenter.fromString(msg);
3031 }
3132 }
3233
3334 public int[] getVector() {
35
3436 int[] vector = new int[] { lastVector[0], lastVector[1], lastVector[2] };
3537 return vector;
38 }
39
40 public void setEnabled(boolean enabled) {
41 this.enabled = enabled;
3642 }
3743
3844 private void calculateVector() {
4046 synchronized (image) {
4147 int[] imageSize = image.getSize();
4248 lastLocation = segmenter.segment(image);
43
44 synchronized (lastVector) {
45 lastVector[0] = lastLocation[0] - imageSize[0] / 2;
46 lastVector[1] = lastLocation[1] - imageSize[1] / 2;
49
50 if (lastLocation != null) {
51 synchronized (lastVector) {
52 lastVector[0] = lastLocation[0] - imageSize[0] / 2;
53 lastVector[1] = lastLocation[1] - imageSize[1] / 2;
54 lastVector[2] = 0;
55 }
56 } else {
57 lastVector = new int[] { 0, 0, ASCEND_VELOCITY };
4758 }
4859 }
4960 }
5162 public void run() {
5263 while (true) {
5364 if (enabled) {
54 if (mBuffer.length != mPic.getBufferLength())
55 mBuffer = new byte[mPic.getBufferLength()];
56 mPic.getBufferCopy(mBuffer);
57 int[] size = mPic.getFrameSize();
58 image.updateImage(mBuffer, size[0], size[1]);
65 if (mBuffer.length != mPic.getBufferLength())
66 mBuffer = new byte[mPic.getBufferLength()];
67 mPic.getBufferCopy(mBuffer);
68 int[] size = mPic.getFrameSize();
69 image.updateImage(mBuffer, size[0], size[1]);
5970 calculateVector();
6071 try {
6172 Thread.sleep(TRACKING_PERIOD_MS);
00 package org.haldean.chopper.pilot;
11
2 import java.io.IOException;
3 import java.io.PrintStream;
4 import java.util.UUID;
5
6 import android.bluetooth.BluetoothAdapter;
7 import android.bluetooth.BluetoothDevice;
8 import android.bluetooth.BluetoothSocket;
9 import android.util.Log;
2 import at.abraxas.amarino.Amarino;
103
114 public class BluetoothOutput {
12 private static String btAddress = "00:06:66:04:B1:BE";
13 private static UUID btUuid = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
5 private static ChopperMain context;
6 private static volatile boolean initialized;
7 static final String BT_DEVICE_ADDR = "00:06:66:04:B1:BE";
8
9 private BluetoothOutput() {
10 // Screw you and your non-utility classes!
11 }
1412
15 private BluetoothSocket btSocket;
16 private PrintStream outputStream;
17
18 public BluetoothOutput() throws IOException {
19 Log.e("ChopperBluetooth", "Starting Bluetooth");
20 BluetoothDevice btDevice = BluetoothAdapter.getDefaultAdapter().getRemoteDevice(btAddress);
21 Log.e("ChopperBluetooth", "Device Acquired");
22 btSocket = btDevice.createRfcommSocketToServiceRecord(btUuid);
23 Log.e("ChopperBluetooth", "Socket Created");
24 btSocket.connect();
25 Log.e("ChopperBluetooth", "And we're connected!");
26 outputStream = new PrintStream(btSocket.getOutputStream());
27 Log.e("ChopperBluetooth", "Output Stream Created");
28 outputStream.print("Hello there, good sir");
29 Log.e("ChopperBluetooth", "Wrote string to output.");
30 setMotorSpeeds(1d, 1d, 1d, 1d);
13 public static void setContext(ChopperMain context) {
14 BluetoothOutput.context = context;
15 initialized = true;
16 }
17
18 public static boolean initialized() {
19 return initialized;
3120 }
3221
33 public void setMotorSpeeds(double m1, double m2, double m3, double m4) {
34 String message = String.format("%03d%03d%03d%03d", m1, m2, m3, m4);
35 Log.e("ChopperBluetooth", "Writing " + message);
36 outputStream.print(message);
22 public static void setMotorSpeeds(double m1, double m2, double m3, double m4) {
23 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));
26 Amarino.sendDataToArduino(context, BT_DEVICE_ADDR, 'D', (int) (100 * m4));
3727 }
38 }
28 }
55 import android.os.PowerManager;
66 import android.view.SurfaceHolder;
77 import android.view.SurfaceView;
8 import at.abraxas.amarino.Amarino;
89 /**
910 * Class to launch all component subroutines
1011 * @author Benjamin Bardin
2829 */
2930 public ChopperMain() {
3031 super();
32 }
33
34 @Override
35 public void onStart() {
36 super.onStart();
37 Amarino.connect(this, BluetoothOutput.BT_DEVICE_ADDR);
38 }
39
40 @Override
41 public void onStop() {
42 super.onStop();
43 Amarino.disconnect(this, BluetoothOutput.BT_DEVICE_ADDR);
3144 }
3245
3346 /**
8699 guid.registerReceiver(comm);
87100
88101 try {
102 BluetoothOutput.setContext(this);
89103 new Thread(comm).start();
90104 new Thread(status).start();
91105 new Thread(reporter).start();
482482 mStatus.setMotorFields(mMotorSpeed);
483483
484484 //Pass motor values to motor controller!
485 BluetoothOutput.setMotorSpeeds(mMotorSpeed[0], mMotorSpeed[1], mMotorSpeed[2], mMotorSpeed[3]);
485486 }
486487
487488 /**
106106 DataReceiver.sendToDefault(taskString);
107107 }
108108
109 public static void fullStop() {
110 setMotorSpeeds(new double[] {0, 0, 0, 0});
111 }
112
109113 /**
110114 * Send PID tuning values to the helicopter.
111115 *
3939 }
4040 });
4141 controlPanel.add(applyButton, 2);
42
43 JButton stopButton = new JButton("Emergency Stop");
44 stopButton.addActionListener(new ActionListener() {
45 public void actionPerformed(ActionEvent e) {
46 EnsignCrusher.fullStop();
47 }
48 });
49 controlPanel.add(stopButton);
50
4251 controlPanel.setBorder(BorderFactory.createEmptyBorder(10,10,10,10));
4352 add(controlPanel, BorderLayout.SOUTH);
4453