Blake Babbs
kind of works
Dependencies: btbee m3pi_ng mbed
Fork of
WarehouseBot
by 
IESS
Revision 0:0f96a93fbf39, committed 2015-06-03
- Comitter:
- charwhit
- Date:
- Wed Jun 03 13:32:30 2015 +0000
- Child:
- 1:2db3ccba18c8
- Commit message:
- Warehouse Bot Code. Trying to get lookup to work
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robot-bae8eb81a9d7/.hg_archival.txt Wed Jun 03 13:32:30 2015 +0000 @@ -0,0 +1,5 @@ +repo: 17669460c6b1bbfb1a779f19ebc29813731580b3 +node: bae8eb81a9d7b89d9998340bd4074944bb8425b1 +branch: default +latesttag: null +latesttagdistance: 4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robot-bae8eb81a9d7/btbee.lib Wed Jun 03 13:32:30 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/ngoldin/code/btbee/#2e759cc06191
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robot-bae8eb81a9d7/m3pi_ng.lib Wed Jun 03 13:32:30 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/jomkippur/code/m3pi_ng/#bfad7a7422fb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot-bae8eb81a9d7/main.cpp Wed Jun 03 13:32:30 2015 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "btbee.h"
+#include "m3pi_ng.h"
+#include <vector>
+#include <string>
+m3pi robot;
+Timer timer;
+Timer time_wait;
+#define MAX 0.75
+#define MIN 0
+
+#define P_TERM 2.5
+#define I_TERM 2
+#define D_TERM 23
+
+void turnRight();
+void turnLeft();
+void goStraight();
+
+int main()
+{
+
+ robot.sensor_auto_calibrate();
+ wait(2.0);
+
+ float right;
+ float left;
+ //float current_pos[5];
+ float current_pos = 0.0;
+ float previous_pos =0.0;
+ float derivative, proportional, integral = 0;
+ float power;
+ float speed = MAX;
+ string turns;
+ int direction = 0;
+ int x [5];
+ bool passed = false;
+
+
+
+ int Run = 1;
+ vector< vector<string> > Lookup(6, vector<string>(6));
+
+ Lookup[1][4] = "LLS";
+
+
+
+
+
+ time_wait.start();
+
+ turns = Lookup[0][1];
+ while(Run) {
+ time_wait.reset();
+ //Get raw sensor values
+
+ robot.calibrated_sensor(x);
+
+
+
+ //Check to make sure battery isn't low
+ if (robot.battery() < 2.4) {
+ timer.stop();
+ break;
+ }
+
+
+
+ if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
+
+ if(turns.at(direction) == 'L'){
+ robot.stop();
+ wait(1);
+ turnLeft();
+ robot.stop();
+ goStraight();
+ wait(1);
+ }
+ else if(turns.at(direction) == 'R'){
+ robot.stop();
+ wait(1);
+ turnRight();
+ robot.stop();
+ goStraight();
+ wait(1);
+ }
+ else{
+ robot.stop();
+ wait(1);
+ goStraight();
+ wait(1);
+ }
+ robot.printf("Size: %d", direction);
+ ++direction;
+ robot.calibrated_sensor(x);
+ }
+ else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
+ robot.stop();
+ break;
+ }
+ else if (x[0]>300 || x[4]>300 && passed)
+ passed = true;
+ else
+ passed = false;
+
+
+ // Get the position of the line.
+ current_pos = robot.line_position();
+ proportional = current_pos;
+
+ derivative = current_pos - previous_pos;
+
+
+ //compute the integral
+ integral =+ proportional;
+
+ //remember the last position.
+ previous_pos = current_pos;
+
+ // compute the power
+ power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+ //computer new speeds
+ right = speed+power;
+ left = speed-power;
+
+ //limit checks
+ if(right<MIN)
+ right = MIN;
+ else if (right > MAX)
+ right = MAX;
+
+ if(left<MIN)
+ left = MIN;
+ else if (left>MIN)
+ left = MAX;
+
+ //set speed
+
+ robot.left_motor(left);
+ robot.right_motor(right);
+
+ wait((5-time_wait.read_ms())/1000);
+ }
+
+
+
+ robot.stop();
+
+ char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
+ ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
+ ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
+ ,'G','8','E','8','D','8','C','4'
+ };
+ int numb = 59;
+
+ robot.playtune(hail,numb);
+
+
+}
+
+
+void turnRight(){
+ Timer turner;
+ turner.start();
+ while (turner.read_ms() < 105){
+ robot.right(.5);
+ }
+ turner.stop();
+}
+
+void turnLeft(){
+ Timer turner;
+ turner.start();
+ while (turner.read_ms() < 105){
+ robot.left(.5);
+ }
+ turner.stop();
+}
+
+void goStraight(){
+ Timer turner;
+ turner.start();
+ while (turner.read_ms() < 50){
+ robot.forward(.5);
+ }
+ turner.stop();
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robot-bae8eb81a9d7/mbed.lib Wed Jun 03 13:32:30 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/screamer/code/mbed/#aff670d0d510