chong wang
A final design project for embedded system class
Diff: main.cpp
- Revision:
- 0:c2c6f1a295f4
diff -r 000000000000 -r c2c6f1a295f4 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 26 17:42:39 2013 +0000
@@ -0,0 +1,215 @@
+
+//#include "HMC6352.h" //compass
+#include "mbed.h"
+//HMC6352 compass(p28, p27);
+Serial pc(USBTX, USBRX);
+AnalogIn Rdist(p19); // right sonar
+AnalogIn Ldist(p20);// left sonar
+AnalogIn Fdist(p18);// front sonar
+AnalogIn range(p17); // right range finder
+Serial device(p9, p10); // tx, rx
+DigitalOut leftlight(LED1);
+DigitalOut rightlight(LED3);
+DigitalOut straightlight(LED2);
+
+// Definitions of iRobot Create OpenInterface Command Numbers
+// See the Create OpenInterface manual for a complete list
+
+
+// Create Command // Arguments
+const char Start = 128;
+const char SafeMode = 131;
+const char FullMode = 132;
+const char Drive = 137; // 4: [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
+const char DriveDirect = 145; // 4: [Right Hi] [Right Low] [Left Hi] [Left Low]
+const char Demo = 136; // 2: Run Demo x
+const char Sensors = 142; // 1: Sensor Packet ID
+const char CoverandDock = 143; // 1: Return to Charger
+const char SensorStream = 148; // x+1: [# of packets requested] IDs of requested packets to stream
+const char QueryList = 149; // x+1: [# of packets requested] IDs of requested packets to stream
+const char StreamPause = 150; // 1: 0 = stop stream, 1 = start stream
+const char PlaySong = 141;
+const char Song = 140;
+/* iRobot Create Sensor IDs */
+const char BumpsandDrops = 7;
+const char Distance = 19;
+const char Angle = 20;
+
+static int speed_left = 200;
+static int speed_right = 200;
+static int state=0;
+//static int arr[10];//0=forwatd, 1=right,2=left,3=finish;
+void start();
+void forward();
+void reverse();
+void left();
+void right();
+void stop();
+void playsong();
+void charger();
+float curveR=0.02;
+float curveL=0.02;
+float Hsouth=98;
+float Hnorth=68;
+float Hwest=80;
+float Heast=77;
+// Demo to move around using basic commands
+int main()
+{
+ int arr[5]= {0,1,0,1,40};//0=forwatd, 1=right,2=left,3=finish;
+ float time=0;
+// wait for Create to power up to accept serial commands
+ wait(3);
+ //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+// set baud rate for Create factory default
+ device.baud(57600);
+// Start command mode and select sensor data to send back
+ start();
+ wait(.5);
+// Move around with motor commands
+ while(1) {
+
+ if((Rdist>0.49 || Ldist>0.49)&&(arr[state]<3)) {
+ leftlight=1;
+ rightlight=1;
+ straightlight=1;
+ if(float(Fdist)<0.12 ) {
+ stop();
+ wait(1);
+ if(float(Fdist)<0.13 ) {
+ state++;
+ if(arr[state]==1) { //right turn
+ speed_right=200;
+ speed_left=200;
+ right();
+ wait(1.0);
+ } else if(arr[state]==2) {
+ speed_right=200;
+ speed_left=200;
+ left();
+ wait(1.0);
+ }//left
+ stop();
+ wait(0.2);
+ forward();
+ wait(6);
+ state++;
+ }
+ }
+
+ // else {
+ // speed_right=200;
+ // speed_left=200;
+ //}
+ } else if((float(Ldist)-float(Rdist))>curveL ) {
+ speed_right++;
+ leftlight=1;
+ rightlight=0;
+ straightlight=0;
+ if(speed_right>206) {
+ speed_right=206;
+ }
+ } else if ( (float(Rdist)-float(Ldist))>curveR ) {
+ speed_left++;
+ leftlight=0;
+ rightlight=1;
+ straightlight=0;
+ if(speed_left>206) {
+ speed_left=206;
+ }
+ }
+
+ else if (((float(Ldist)-float(Rdist))<curveL) && ( (float(Rdist)-float(Ldist))<curveR )) {
+ speed_left=200;
+ speed_right=200;
+ leftlight=0;
+ rightlight=0;
+ straightlight=1;
+ }
+ if(arr[state]>2) {
+ time=time+0.1;
+ if(time>arr[state]) {
+ break;//while
+ }
+ }
+
+ //pc.printf("Left: %f ", float(Ldist));
+ pc.printf("Right: %f ", float(Rdist));
+ pc.printf("range: %f ", float(range));
+ //pc.printf("front: %f ", float(Fdist));
+ //pc.printf("Diff: %f\n", (float(Rdist)-float(Ldist)));
+ forward();
+ wait(0.1);
+ }
+// Play a song
+ stop();
+ playsong();
+ wait(10);
+// Search for battery charger IR beacon
+ // charger();
+}
+
+
+// Start - send start and safe mode, start streaming sensor data
+void start()
+{
+ // device.printf("%c%c", Start, SafeMode);
+ device.putc(Start);
+ device.putc(SafeMode);
+ wait(.5);
+ // device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
+ device.putc(SensorStream);
+ device.putc(1);
+ device.putc(BumpsandDrops);
+ wait(.5);
+}
+// Stop - turn off drive motors
+void stop()
+{
+ device.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0));
+}
+// Forward - turn on drive motors
+void forward()
+{
+ device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
+ char((speed_left>>8)&0xFF), char(speed_left&0xFF));
+
+}
+// Reverse - reverse drive motors
+void reverse()
+{
+ device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
+ char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
+
+}
+// Left - drive motors set to rotate to left
+void left()
+{
+ device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
+ char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
+}
+// Right - drive motors set to rotate to right
+void right()
+{
+ device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
+ char((speed_left>>8)&0xFF), char(speed_left&0xFF));
+
+}
+// Charger - search and return to charger using IR beacons (if found)
+void charger()
+{
+ device.printf("%c%c", Demo, char(1));
+}
+// Play Song - define and play a song
+void playsong() // Send out notes & duration to define song and then play song
+{
+
+ device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
+ Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
+ char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
+ char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
+ char(24), char(86), char(12), char(87), char(48));
+
+ wait(.2);
+ device.printf("%c%c", PlaySong, char(0));
+}
\ No newline at end of file