Revision 4:67d7177c213f, committed 2017-04-19

Comitter:

beacon

Date:

Wed Apr 19 12:23:52 2017 +0000

Parent:

3:fa61be4ecaa6

Child:

5:1aaf5de776ff

Commit message:

l;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/Declarations.h	Wed Apr 19 12:23:52 2017 +0000
@@ -0,0 +1,38 @@
+#ifndef DECLARATIONS_H
+#define DECLARATIONS_H
+
+//DistanceSensors:
+#define NEAR 0.18f          //Used for distance Sensors. If they're to near to a wall -> turn
+#define NEAR_LEGO 0.20f     //If the DistanceSensors are near to a Lego...
+
+#define LEFT_L 5            //Arrayindex of the left LEGO Sensor & left LED
+#define FWD_L 0             //Arrayindex of the front LEGO Sensor & front LED
+#define RIGHT_L 1           //Arrayindex of the right LEGO Sensor & right LED
+
+#define LEFT 7              //Arrayindex of the left Sensor
+#define FWD 6               //Arrayindex of the front Sensor
+#define RIGHT 8             //Arrayindex of the right Sensor
+#define BRIGHT 2            //Arrayindex of the backward right Sensor & left LED
+#define BWD 3               //Arrayindex of the backward Sensor & front LED
+#define BLEFT 4             //Arrayindex of the backsward left Sensor & right LED
+
+//ColorSensors:
+#define RED_UPLIMIT 500     //Default limit in mV
+#define GREEN_DOWNLIMIT 501 //
+#define GREEN_UPLIMIT 1200  //
+
+#define GREEN 1             //Will be used to operate arm functions
+#define NOBLOCK 0           //
+#define RED -1              //
+
+//Greifer:
+#define PC_7 SERVO0
+
+//Arm:
+#define BACK 80             //Angle to set the arm up and back
+
+//Misc:
+#define TIMEOUT 140         //if the timer reaches TIMEOUT ([TIMEOUT] = 0.1s), the robot will reset.
+#define MAX 50              //Once the counter reaches MAX, it will turn around.
+
+#endif
\ No newline at end of file

--- a/Headers/Robot.h	Wed Mar 22 13:33:07 2017 +0000
+++ b/Headers/Robot.h	Wed Apr 19 12:23:52 2017 +0000
@@ -3,26 +3,10 @@
 
 #include <cstdlib>
 #include <mbed.h>
+#include "Servo.h"
 
 #include "mbed.h"
 
-//DistanceSensors:
-#define NEAR 0.1f           //Used for distance Sensors. If they're to near to a wall -> turn
-#define LEFT 5              //Arrayindex of the left Sensor & left LED
-#define FWD 0               //Arrayindex of the front Sensor & front LED
-#define RIGHT 1             //Arrayindex of the right Sensor & right LED.
-
-//ColorSensors:
-#define RED_UPLIMIT 500     //Default limit in mV
-#define GREEN_DOWNLIMIT 501 //Default limit in mV
-#define GREEN_UPLIMIT 1200  //Default limit in mV
-#define GREEN 1
-#define NOBLOCK 0
-#define RED -1
-
-//Misc:
-#define TIMEOUT 5
-
 
 /* Declarations for the Motors in the Robot in order to drive -------- */
 
@@ -30,10 +14,10 @@
 {
     public:
         
-        DistanceSensors(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
+        DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
         DistanceSensors();
         
-        void init(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
+        void init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
         virtual ~DistanceSensors();    
         float read();
         
@@ -42,6 +26,10 @@
     private:
         
         AnalogIn*       sensorVoltage;
+        AnalogIn*       frontS;
+        AnalogIn*       leftS;
+        AnalogIn*       rightS;
+        
         DigitalOut*     bit0;
         DigitalOut*     bit1;
         DigitalOut*     bit2;
@@ -65,6 +53,24 @@
     
         AnalogIn*   FarbVoltage;
 };
+/*
+class Arm{
+    
+    public:
+        Arm(Servo joint);
+        Arm();
+        void init(Servo joint);
+        
+        void down();
+        void neutral();
+        void back();
+        void setAngle(float angle);
+        
+    private:
+        Servo joint;
+        
+};
+*/
 
 class Robot
 {
@@ -72,17 +78,40 @@
     public:
         
         //Robot related:
-        Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage );
+        Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage,  AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS );
+        
+        //Drive Functions
         void drive();
+        void driveB();
+
         void turnLeft();
+        void turnLeftS();
         void turnRight();
+        void turnRightS();
         void turnAround(int left);
         void stop();
         
-        void search(int* counter, Timer* t);
+        //Functions that use the drive functions
+        void wallRight(int* counter, int* timer, int* lastAct);
+        void wallLeft(int* counter, int* timer, int* lastAct);
+        void wallFront(int* counter, int* timer, int* lastAct);
+        void counterMax(int* counter, int* timer, int* lastAct, int* rando);
+        
+        void legoFront(int* counter, int* timer, int* lastAct, int* legoFound);
+        void legoRight(int* counter, int* timer, int* lastAct, int* legoFound);
+        void legoLeft(int* counter, int* timer, int* lastAct, int* legoFound);
+        
+        void nothingFound(int* counter, int* timer, int* lastAct);
+        
+        void search(int* counter, int* timer, int* found);
+        void lego(int* counter, Timer* t);
         
         //DistanceSensors related:
-        DistanceSensors sensors[6];
+        DistanceSensors sensors[9];
+        AnalogIn* frontS;
+        AnalogIn* leftS;
+        AnalogIn* rightS;
+        
         
         void initializeDistanceSensors();
         //void init();
@@ -103,6 +132,8 @@
         DigitalIn*      errorSignal;
         DigitalIn*      overtemperatur;
         
+        Servo*          arm;
+        
 
 };
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Apr 19 12:23:52 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Arm.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -0,0 +1,30 @@
+/*#include "Robot.h"
+
+Arm::Arm(Servo joint){
+    init(joint);
+}
+
+Arm::Arm(){
+    
+}
+
+void Arm::init(Servo joint){
+    this.joint = joint;
+}
+
+void Arm::down(){
+    //do stuff
+}
+
+void Arm::neutral(){
+    //do stuff
+}
+
+void Arm::back(){
+    //do stuff
+}
+
+void Arm::setAngle(float angle){
+    //do stuff
+}
+*/
\ No newline at end of file

--- a/Sources/DistanceSensors.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/DistanceSensors.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,5 +1,6 @@
 #include <cmath>
 #include "Robot.h"
+#include "Declarations.h"
 
 
 
@@ -8,15 +9,19 @@
     //Nothing
 }
     
-DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
 {   
-    init(sensorVoltage, bit0, bit1, bit2, number);
+    init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number);
 }
 
 //initialise
-void DistanceSensors::init(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
 {
     this->sensorVoltage = sensorVoltage;
+    this->frontS = frontS;
+    this->leftS = leftS;
+    this->rightS = rightS;
+    
     this->bit0 = bit0;
     this->bit1 = bit1;
     this->bit2 = bit2;
@@ -30,12 +35,32 @@
 
 
 float DistanceSensors::read()//Return the distance of an object
-{  
-    *bit0 = number & 1; // Set the first bit of the Sensors MUX
-    *bit1 = number & 2; // Set the second bit of the Sensors MUX
-    *bit2 = number & 4; // Set the third bit of the Sensors MUX
+{
     
-    float Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor   
+    float Usensor;
+  
+    if (number < 6){
+        *bit0 = number & 1; // Set the first bit of the Sensors MUX
+        *bit1 = number & 2; // Set the second bit of the Sensors MUX
+        *bit2 = number & 4; // Set the third bit of the Sensors MUX
+        
+        Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
+    }
+    else{
+        switch(number){
+            case 6:
+                Usensor=frontS->read();
+                break;
+            case 7:
+                Usensor=leftS->read();
+                break;
+            case 8:
+                Usensor=rightS->read();
+                break;
+        }
+    }
+    
+    //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
     float Distance=-0.38f*sqrt(Usensor)+0.38f;
     return Distance;
 }

--- a/Sources/Farbsensor.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/Farbsensor.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,4 +1,5 @@
 #include "Robot.h"
+#include "Declarations.h"
 
 
 Farbsensor::Farbsensor()

--- a/Sources/Robot.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/Robot.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,8 +1,9 @@
 #include "Robot.h"
+#include "Declarations.h"
 
 /* Work in progress -------------------------------------------- */
 
-Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage )
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS )
 {
     this->powerSignal = enableSignal;
     this->left =        left;
@@ -14,10 +15,15 @@
     this->leds =        leds;
     
     this->FarbVoltage = FarbVoltage;
+    this->frontS =      frontS;
+    this->leftS =       leftS;
+    this->rightS =      rightS;
+    
+    this->arm =         arm;
 
 }
 
-/* work in progress ------------------------------------------- */
+//Drive functions
 void Robot::drive()
 {
     //pwm determine what direction it goes.
@@ -26,6 +32,14 @@
     *right= 0.4f;
 }
 
+void Robot::driveB()
+{
+    //pwm determine what direction it goes.
+    *powerSignal = 1;
+    *left=  0.4f;
+    *right= 0.6f;
+}
+
 void Robot::turnLeft()
 {
     
@@ -35,6 +49,15 @@
 
 }
 
+void Robot::turnLeftS()
+{
+    
+    *powerSignal = 1;
+    *left=  0.45f;
+    *right= 0.45f;
+
+}
+
 void Robot::turnRight()
 {
     *powerSignal = 1;
@@ -42,6 +65,15 @@
     *right= 0.6f;
 }
 
+void Robot::turnRightS()
+{
+    
+    *powerSignal = 1;
+    *left=  0.55f;
+    *right= 0.55f;
+
+}
+
 void Robot::turnAround(int left)
 {
     *powerSignal = 1;
@@ -62,67 +94,238 @@
     *right= 0.5f;
 }
 
-void Robot::search(int* counter, Timer* t){
-    int rando;
+
+//Functions that use the drive functions
+void Robot::wallRight(int* counter, int* timer, int* lastAct){
+    *counter += 1;
+        
+    if (*lastAct != 1){             //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 1;
+    }
+        
+    this->turnLeft();
+}
+
+void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){
+    if (*lastAct != 0){                 //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 0;
+    }
+    
+    if (*rando == -1){                  //If rando was unused, set a new number.
+        *rando = rand() % 2;
+    }
+    
+    if (this->sensors[FWD] < NEAR){   //While something is seen turn around.
+        this->turnAround(*rando);
+    }
+    
+    else{
+        *rando = -1;
+        *counter = 0;
+    }
+}
+
+void Robot::wallLeft(int* counter, int* timer, int* lastAct){
+    *counter += 1;
+        
+    if (*lastAct != 2){             //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 2;
+    }
+        
+    this->turnRight();
+}
+
+void Robot::wallFront(int* counter, int* timer, int* lastAct){
+    if (*lastAct != 3){              //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 3;
+    }
+        
+    *counter = MAX;                   //By setting the counter to MAX, next time it will go into the first if-statement (action 0).
+}
+
+
+void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound){
+    //*counter += 1;
+    *legoFound = 0;
+    
+    if (*lastAct != 4){               //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 4;
+    }
+    
+    if (this->sensors[FWD] < NEAR){
+        *legoFound = -1;
+    }
+    
+    else if (this->sensors[FWD_L] > 0.15f){
+        this->drive();
+    }
+    else{
+        this->stop();
+        *legoFound = 3;
+    }
+}
+
+void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
+    //*counter += 1;
+    *legoFound = 1;
+    
+    if (*lastAct != 5){          //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 5;
+    }
+    
+    if (this->sensors[FWD_L] > NEAR){
+        this->turnRight();
+    }
+    else{
+        this->stop();
+        *legoFound = 3;
+    }
+}
+
+void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
+    //*counter += 1;
+    *legoFound = 2;
     
-    if (*counter >= 5) {
-        //t->reset();
+    if (*lastAct != 6){          //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 6;
+    }
+    
+    if (this->sensors[FWD_L] > NEAR){
+        this->turnLeft();
+    }
+    else{
+        this->stop();
+        *legoFound = 3;
+    }
+}
+
+
+void Robot::nothingFound(int* counter, int* timer, int* lastAct){
+    *counter = 0;    
+    if (*lastAct != 7){          //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 7;
+    }
+    
+    this->drive();
+}
+
+
+void Robot::search(int* counter, int* timer, int* found){
+    
+    static int rando = -1;          //Rando will be used, to randomize turnAround()
+                                    //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
+    
+    static int lastAct = -1;        //Is used, to check if the same action in Robot::search() was made multiple times.
+                                    //-1 := unused; x != -1 := action x was called last.
+                                    
+    static int legoFound = -1;      //Is used to determine, on what side the lego was found.
+                                    //-1 := unused; 0:= front; 1:= right, 2:= left.
+                                    
+    *timer += 1;                    //timer holds the time in 0.1s
+    
+    
+    if (*counter >= MAX/* && legoFound == -1*/) {                   //Robot is stuck turning left & right
+        counterMax(counter, timer, &lastAct, &rando);
+    }
+    
+    //Wall actions:
+    else if (this->sensors[RIGHT] < NEAR && legoFound == -1){   //Robot has spotted an obstacle on the right.
+        wallRight(counter, timer, &lastAct);
+    }
+    
+    else if (this->sensors[LEFT] < NEAR && legoFound == -1) {   //Robot has spotted an obstacle on the left.
+        wallLeft(counter, timer, &lastAct);
+    }
+    
+    else if (this->sensors[FWD] < NEAR && legoFound == -1)  {   //Robot has spotted an obstacle infront
+        wallFront(counter, timer, &lastAct);
+    }
+    
+    //Lego actions:
+    else if (this->sensors[FWD_L] < NEAR_LEGO && legoFound == -1 || legoFound == 0){    //There's a Lego in front.
+        legoFront(counter, timer, &lastAct, &legoFound);
+    }
+
+    else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){  //There's a Lego on the right.
+        legoRight(counter, timer, &lastAct, &legoFound);
+    }
+    
+    else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){   //There's a Lego on the left.
+        legoLeft(counter, timer, &lastAct, &legoFound);
+    }
+    
+    //Lego is ready to be grabbed.
+    else if(legoFound == 3){
+        *found = 1;             //When found is set to 1, main will call arms functions
+        *counter = 0;
+        *timer = 0;
+        legoFound = -1;
         
-        rando = rand() % 2;
-        while (this->sensors[FWD] < NEAR){
+        //Old! Check if still useful!{
+            //check if this was the last action
+                //reset timer
+    
+            //dont add up counter
+            //use arm
+            //set legoFound to -1
+        //}
+    }
+    
+    
+    //Nothing found
+    else {
+        nothingFound(counter, timer, &lastAct);
+    }
+}
+
+void Robot::lego(int* counter, Timer* t){
+    
+    if (this->sensors[RIGHT_L] < NEAR_LEGO){
+        t->reset();
+        
+        *counter += 1;
+        while (this->sensors[FWD] > NEAR_LEGO){
             if ( t->read() > TIMEOUT ){
                 break;
             }
             
-            this->leds[FWD] = 1;
-            this->turnAround(rando);
+            this->turnRight();
         }
-        this->leds[FWD] = 0;   
+        this->stop();
     }
     
-    else if (this->sensors[RIGHT] < NEAR){
+    else if (this->sensors[LEFT] < NEAR_LEGO){
         t-> reset();
         
         *counter += 1;
-        while (this->sensors[RIGHT] < NEAR){
+        while (this->sensors[FWD] > NEAR_LEGO){
             if ( t->read() > TIMEOUT ){
                 break;
             }
             
             this->turnLeft();
-            this->leds[RIGHT] = 1;
         }
-        this->leds[RIGHT] = 0;
+        this->stop();
     }
     
-    else if (this->sensors[LEFT] < NEAR){
-        t-> reset();
-        
-        *counter += 1;
-        while (this->sensors[LEFT] < NEAR){
-            if ( t->read() > TIMEOUT ){
-                break;
-            }
-            
-            this->turnRight();
-            this->leds[LEFT] = 1;
+    else if (this->sensors[FWD] < 0.12f){
+        while (this->sensors[FWD] < 0.12f){
+            this->driveB();
         }
-        this->leds[LEFT] = 0;
-    }
-    
-    else if (this->sensors[FWD] < NEAR) {
-        t-> reset();
-        
-        rando = rand() % 2;
-        while (this->sensors[FWD] < NEAR){
-            if ( t->read() > TIMEOUT ){
-                break;
-            }
-            
-            this->leds[FWD] = 1;
-            this->turnAround(rando);
+        while (this->sensors[FWD] < NEAR_LEGO){
+            this->turnRight();
         }
-     this->leds[FWD] = 0;   
+        while(1){
+            this->stop();
+        }
     }
     
     else {

--- a/Sources/main.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/main.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "Robot.h"
+#include "Declarations.h"
 
 #include <cstdlib>
 
-//DistanceSensors related:
+//DistanceSensors related bottom:
 Serial pc(SERIAL_TX, SERIAL_RX);
 
 AnalogIn sensorVoltage(PB_1);
@@ -12,6 +13,11 @@
 DigitalOut bit1(PC_2);
 DigitalOut bit2(PC_3);
 
+//DistanceSensors top:
+AnalogIn frontS(A1);
+AnalogIn leftS(A2);
+AnalogIn rightS(A3);
+
 //Leds related:
 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
 
@@ -23,14 +29,17 @@
 DigitalIn       errorSignal(PB_14);
 DigitalIn       overtemperatur(PB_15);
 
+//Arm:
+//Servo arm(PC_7);
+
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
 
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage ); //Implement the Farbsensor into the Robot init function!!
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS ); //Implement the Farbsensor into the Robot init function!!
 
 void initializeDistanceSensors(){
-    for( int ii = 0; ii<6; ++ii) {
-        sam.sensors[ii].init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
+    for( int ii = 0; ii<9; ++ii) {
+        sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
 
     enable = 1;
     }
@@ -38,19 +47,57 @@
 
 /* */
 int main(){
-    Timer t;
-    t.start();
+    initializeDistanceSensors();        //Initialises IR-Sensors.
+    int counter = 0;                    //Counts how many times the robot has turned, before driving
+    int timer = 0;                      //Is used, to reset the robot. Returns time in [0.1s]
+    //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
+    int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
     
-    initializeDistanceSensors();
-    int counter = 0;                //counts how many times the robot has turned, before driving
+    while( 1 ){
+        if ( timer > TIMEOUT ){
+            NVIC_SystemReset();         //Resets Sam.
+        }
+        else if (found == 0){
+            sam.search(&counter, &timer, &found);
+        }
+        else{
+            //pick up lego
+            //found = 0;
+        }
         
-    while( 1 ){
-        if ( t.read() > TIMEOUT ){
-            NVIC_SystemReset();     //Resets Sam.
-        }
-            
-        sam.search(&counter, &t);
         wait( 0.1f );
     }
     return 0;
-}
\ No newline at end of file
+}
+
+/* * /
+int main(){
+    
+    initializeDistanceSensors();
+    sam.stop();
+    
+    while ( 1 ){
+        for (int i=0; i<6; i++){
+            sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
+        }
+        
+        sam.turnLeftS();
+        
+        while (sam.sensors[FWD] < NEAR){
+            wait(0.05f);
+            sam.stop();
+        }
+        
+    }
+    return 0;
+}
+/* */
+
+/* * /
+int main(){
+    for(float p=0; p<1.0; p += 0.1) {
+      //  arm = p;
+        wait(0.2);
+    }
+}
+/* */
\ No newline at end of file