![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
that thing for testing
Dependencies: DistanceSensor Gripper R5_StepperDrive mbed
Diff: GrabTest.cpp
- Revision:
- 0:345761c4cf73
diff -r 000000000000 -r 345761c4cf73 GrabTest.cpp --- /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