Yo Fleyt
with class
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Revision 0:8bffb51cc345, committed 2017-03-21
- Comitter:
- geotsam
- Date:
- Tue Mar 21 15:24:34 2017 +0000
- Child:
- 1:f0807d5c5a4b
- Commit message:
- Working
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ISR_Mini-explorer.lib Tue Mar 21 15:24:34 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/ISR/code/ISR_Mini-explorer/#15a30802e719
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 21 15:24:34 2017 +0000
@@ -0,0 +1,69 @@
+#include "mbed.h"
+#include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
+#include "math.h"
+
+float dist(float robot_x, float robot_y, float target_x, float target_y);
+
+int main(){
+ initRobot();
+ pc.baud(9600); // baud for the pc communication
+
+ float target_x=46.8, target_y=78.6;
+
+ float angle_error; //angle error
+ float d; //distance from target
+ float k_linear=10, k_angular=200;
+ float linear, angular, angular_left, angular_right;
+
+ float r=3.25, b=7.2;
+
+ int speed=999;
+
+ theta=0;
+ X=0;
+ Y=0;
+
+ do {
+ pc.printf("\n\n\r entered while");
+
+ Odometria();
+
+ angle_error = atan2((target_y-Y),(target_x-X))-theta;
+ angle_error = atan(sin(angle_error)/cos(angle_error));
+ d=dist(X, Y, target_x, target_y);
+ pc.printf("\n\r d=%f", d);
+
+ linear=k_linear*d;
+ angular=k_angular*angle_error;
+ angular_left=(linear-0.5*b*angular)/r;
+ angular_right=(linear+0.5*b*angular)/r;
+
+ if (d<25) {
+ speed = d*30;
+ }
+ if(angular_left>angular_right) {
+ angular_right=speed*angular_right/angular_left;
+ angular_left=speed;
+ } else {
+ angular_left=speed*angular_left/angular_right;
+ angular_right=speed;
+ }
+
+ pc.printf("\n\r X=%f", X);
+ pc.printf("\n\r Y=%f", Y);
+
+ leftMotor(1,1*angular_left);
+ rightMotor(1,1*angular_right);
+
+ wait(0.5);
+ } while(d>1);
+
+ leftMotor(1,0);
+ rightMotor(1,0);
+
+ pc.printf("\n\r %f -- arrived!", d);
+}
+
+float dist(float robot_x, float robot_y, float target_x, float target_y){
+ return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Mar 21 15:24:34 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90 \ No newline at end of file