Revision 1:7bf9b6c007a1, committed 2018-03-23

Comitter:

kueenste

Date:

Fri Mar 23 15:40:09 2018 +0000

Parent:

0:7cf5bf7e9486

Commit message:

schissdreck funktioniert so halbe

Changed in this revision

--- a/Library/Controller.cpp	Fri Mar 23 13:07:58 2018 +0000
+++ b/Library/Controller.cpp	Fri Mar 23 15:40:09 2018 +0000
@@ -40,12 +40,12 @@
     
     // initialize local variables
     
-    translationalMotion.setProfileVelocity(1.5f);
-    translationalMotion.setProfileAcceleration(1.5f);
+    translationalMotion.setProfileVelocity(0.5f);
+    translationalMotion.setProfileAcceleration(0.5f);
     translationalMotion.setProfileDeceleration(3.0f);
     
-    rotationalMotion.setProfileVelocity(3.0f);
-    rotationalMotion.setProfileAcceleration(15.0f);
+    rotationalMotion.setProfileVelocity(1.5f);
+    rotationalMotion.setProfileAcceleration(5.0f);
     rotationalMotion.setProfileDeceleration(15.0f);
     
     translationalVelocity = 0.0f;

--- a/Library/StateMachine.cpp	Fri Mar 23 13:07:58 2018 +0000
+++ b/Library/StateMachine.cpp	Fri Mar 23 15:40:09 2018 +0000
@@ -17,7 +17,7 @@
 /**
  * Creates and initializes a state machine object.
  */
-StateMachine::StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5) : controller(controller), enableMotorDriver(enableMotorDriver), led0(led0), led1(led1), led2(led2), led3(led3), led4(led4), led5(led5), button(button), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3), irSensor4(irSensor4), irSensor5(irSensor5) {
+StateMachine::StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5, DigitalOut& led) : controller(controller), enableMotorDriver(enableMotorDriver), led0(led0), led1(led1), led2(led2), led3(led3), led4(led4), led5(led5), button(button), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3), irSensor4(irSensor4), irSensor5(irSensor5), led(led) {
     
     enableMotorDriver = 0;
     state = ROBOT_OFF;
@@ -71,8 +71,19 @@
                 
                 enableMotorDriver = 1;
                 
-                controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
-                controller.setRotationalVelocity(0.0f);
+                // Now it's time for some tasks 
+                //controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                //controller.setRotationalVelocity(0.0f);
+                
+                // Plan some tasks
+                
+                // TASKWAIT FUNKTIONIERT BI MIR NID
+                taskList.push_back(new TaskMoveTo(controller, 0.0f, 1.0f, 0.0f));
+                //taskList.push_back(new TaskWait(controller, 1.0f));
+                taskList.push_back(new TaskMoveTo(controller, 1.0f, 1.0f, 0.0f));
+                
+                taskList.push_back(new TaskMoveTo(controller, 0.0f, 1.0f, 0.0f));
+                taskList.push_back(new TaskMoveTo(controller, 0.0f, 0.0f, 0.0f));
                 
                 state = MOVE_FORWARD;
             }
@@ -82,6 +93,21 @@
             break;
             
         case MOVE_FORWARD:
+            led = 1;
+            if (taskList.size() > 0) {
+                Task* task = taskList.front();
+                if (task->run(0.0f) == Task::DONE) {
+                    led = 0;
+                    taskList.pop_front();
+                    delete task;
+                }
+            } else {
+                
+                controller.setTranslationalVelocity(0.0f);
+                controller.setRotationalVelocity(0.0f);
+                
+                state = SLOWING_DOWN;    
+            }
             
             buttonNow = button;
             
@@ -161,6 +187,11 @@
                 
                 enableMotorDriver = 0;
                 
+                while (taskList.size() > 0) {
+                    delete taskList.front();
+                    taskList.pop_front();
+                }
+                
                 state = ROBOT_OFF;
             }
             

--- a/Library/StateMachine.h	Fri Mar 23 13:07:58 2018 +0000
+++ b/Library/StateMachine.h	Fri Mar 23 15:40:09 2018 +0000
@@ -12,6 +12,11 @@
 #include "Controller.h"
 #include "IRSensor.h"
 
+#include <deque>
+#include "Task.h"
+#include "TaskWait.h"
+#include "TaskMoveTo.h"
+
 /**
  * This class implements a simple state machine for a mobile robot.
  * It allows to move the robot forward, and to turn left or right,
@@ -28,7 +33,7 @@
         static const int    TURN_RIGHT = 3;
         static const int    SLOWING_DOWN = 4;
         
-                    StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5);
+                    StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5,DigitalOut& led);
         virtual     ~StateMachine();
         int         getState();
         
@@ -41,6 +46,7 @@
         
         Controller&     controller;
         DigitalOut&     enableMotorDriver;
+        DigitalOut&     led;
         DigitalOut&     led0;
         DigitalOut&     led1;
         DigitalOut&     led2;
@@ -59,6 +65,8 @@
         int             buttonBefore;
         Ticker          ticker;
         
+        deque<Task*> taskList;
+        
         void            run();
 };
 

--- a/Library/TaskMoveTo.cpp	Fri Mar 23 13:07:58 2018 +0000
+++ b/Library/TaskMoveTo.cpp	Fri Mar 23 15:40:09 2018 +0000
@@ -10,7 +10,7 @@
 using namespace std;
 
 const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f;    // default velocity value, given in [m/s]
-const float TaskMoveTo::DEFAULT_ZONE = 0.01f;       // default zone value, given in [m]
+const float TaskMoveTo::DEFAULT_ZONE = 0.1f;       // default zone value, given in [m]
 const float TaskMoveTo::PI = 3.14159265f;           // the constant PI
 const float TaskMoveTo::K1 = 2.0f;                  // position controller gain parameter
 const float TaskMoveTo::K2 = 0.5f;                  // position controller gain parameter
@@ -23,8 +23,9 @@
  * @param y the y coordinate of the target position, given in [m].
  * @param alpha the target orientation, given in [rad].
  */
-TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
-    
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller)
+{
+
     this->x = x;
     this->y = y;
     this->alpha = alpha;
@@ -40,8 +41,9 @@
  * @param alpha the target orientation, given in [rad].
  * @param velocity the maximum translational velocity, given in [m/s].
  */
-TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
-    
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller)
+{
+
     this->x = x;
     this->y = y;
     this->alpha = alpha;
@@ -58,8 +60,9 @@
  * @param velocity the maximum translational velocity, given in [m/s].
  * @param zone the zone threshold around the target position, given in [m].
  */
-TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
-    
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller)
+{
+
     this->x = x;
     this->y = y;
     this->alpha = alpha;
@@ -77,10 +80,40 @@
  * @param period the period of the task sequencer, given in [s].
  * @return the status of this task, i.e. RUNNING or DONE.
  */
-int TaskMoveTo::run(float period) {
+int TaskMoveTo::run(float period)
+{
+
+    // ignore period (only because of Task.h vererbung)
+
+    // bitte implementieren!
+
+    float xtargminx = x-controller.getX();
+    float ytargminy = y-controller.getY();
+
+    float roh = sqrt(pow(xtargminx,2)+pow(ytargminy,2));
+    float gamma = atan2(ytargminy,xtargminx) - controller.getAlpha();
+    float delta = gamma+controller.getAlpha() - alpha ;
     
-    // bitte implementieren!
-    
-    return RUNNING;
+    if(gamma > PI) {
+        gamma -= 2*PI;       
+    } else if(gamma < -PI) {
+        gamma += 2*PI;    
+    }
+    
+    if(delta > PI) {
+        delta -= 2*PI;       
+    } else if(delta < -PI) {
+        delta += 2*PI;    
+    }
+
+    controller.setTranslationalVelocity(K1*roh*cos(gamma));
+    controller.setRotationalVelocity(K2*gamma+K1*sin(gamma)*cos(gamma)*(gamma+K3*delta)/gamma);
+
+    if(abs(xtargminx)<=zone && abs(ytargminy)<=zone && abs(alpha-controller.getAlpha())<=zone) {
+        return DONE;
+    } else {
+        return RUNNING;
+    }
+
 }
 

--- a/Main.cpp	Fri Mar 23 13:07:58 2018 +0000
+++ b/Main.cpp	Fri Mar 23 15:40:09 2018 +0000
@@ -57,7 +57,7 @@
     // create robot controller objects
     
     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
-    StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
+    StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5, led);
     
     while (true) {