Texas State IEEE
that thing for testing
Dependencies: DistanceSensor Gripper R5_StepperDrive mbed
Revision 0:345761c4cf73, committed 2016-04-09
- Comitter:
- Hypna
- Date:
- Sat Apr 09 04:27:41 2016 +0000
- Commit message:
- Initial publish
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DistanceSensor.lib Sat Apr 09 04:27:41 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/Texas-State-IEEE-Robotics/code/DistanceSensor/#6ebd72287e97
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GrabTest.cpp Sat Apr 09 04:27:41 2016 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "DistanceSensor.h"
+#include "Gripper.h"
+#include "R5_StepperDrive.h"
+#include <cmath>
+
+int main()
+{
+ Serial bluetooth(PTE16,PTE17);
+ DistanceSensor dsRight(PTC2,3);
+ DistanceSensor dsLeft(PTC1,1);
+ Gripper grip(PTE20,PTE21);
+ StepperDrive drive(bluetooth,PTE19,PTE18,1,PTE3,PTE2,PTE22,0,10.0625,8.1875,700);
+ double lDist, rDist;
+ const int readThresh = 10;
+ const double pi = 3.14159;
+ const double theta = pi/4; //scan servo angle in radians
+ const int k1 = x; //distance from plane of the sensors to center of rotation
+ const int k2 = x; //distance from sensors to center line
+ double a, b, c, A, B, adjust, dist;
+ char side;
+
+ bluetooth.printf("Beginning Search\n\r");
+ while(true)
+ {
+ lDist = dsLeft.getDistance();
+ rDist = dsRight.getDistance();
+
+ if(lDist < readThresh || rDist < readThresh)
+ break;
+
+ drive.move(2,0);
+ }
+
+ bluetooth.printf("Range Found\n\r");
+
+ if(lDist < rDist)
+ {
+ c = lDist;
+ side = 'l';
+ bluetooth.printf("Using Left Distance %d\n\r", c);
+ }
+ else
+ {
+ c = rDist;
+ side = 'r';
+ bluetooth.printf("Using Left Distance %d\n\r", c);
+ }
+
+ a = c*sin(theta);
+ b = sqrt(pow(c,2) - pow(a,2)); //sqrt(c^2 - a^2)
+ A = a + k1;
+
+ if(side == 'l')
+ B = b - k2;
+ else
+ B = k2 - b;
+
+ adjust = pi/2 - atan(A/B);
+ dist = sqrt(pow(A,2) + pow(B,2));
+
+ bluetooth.printf("Approach Move is %f at %f rads\n\r", dist, adjust);
+
+ drive.move(dist,adjust);
+ while(!drive.isMoveDone())
+ wait_us(5);
+
+ bluetooth.printf("Move Complete\n\r");
+ bluetooth.printf("Grabbing\n\r");
+ grip.grip();
+ grip.lift();
+ bluetooth.printf("Done!\n\r");
+
+ return 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Gripper.lib Sat Apr 09 04:27:41 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/Texas-State-IEEE-Robotics/code/Gripper/#92a09a39e546
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/R5_StepperDrive.lib Sat Apr 09 04:27:41 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/Texas-State-IEEE-Robotics/code/R5_StepperDrive/#ceb1201fd3a6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Apr 09 04:27:41 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68 \ No newline at end of file