/* 
 * Thomas Pantzer: gripper_demo.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1999 by Thomas Pantzer
 * 
 * Permission to use, copy, modify, and distribute this software for noncommercial 
 * use and without fee is hereby granted, provided that the above copyright notice 
 * appear in all copies and that both that copyright notice and this permission 
 * notice appear in supporting documentation, and that the name of the Author not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The Author makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@isys2.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */




// #include "t_runtime_obj.h"

#include <unistd.h>
#include <stdlib.h>

#include "copyright.h"

#include "trigonom.h"


#define _GRIPPER_DEMO_G
#include "gripper_demo.h"


// move this to an appropriate location
int get_random(int l,int u)
{
   int i;
   i = l + (int)((1.0*u)*rand()/(RAND_MAX+1.0));

   return i;
}

// other imperative controllers might want to access this routine
void Go_Away(RobotContext *Rob,int dir,int tol)
{
   int i,j,l,u;
   
   l = dir - tol;
   u = dir + tol;

   Rob->ReadWheelCounter(&i,&j);
   
   if (i>j)
        Rob->Turn(get_random(l,u));   
     else
        Rob->Turn(-get_random(l,u));  
}





char gripper_boxes[1][32] = {"links abwerfen\0"};
// char gripper_sliders[2][16] = {"uno\0","dos\0"};

void t_gripper_demo::Init(RobotContext *robot)
{
	char *cb_pointers[] = {&gripper_boxes[0][0]}; 
//,&dummyboxes[1][0],&dummyboxes[2][0]};
//	char *sl_pointers[] = {&dummysliders[0][0],&dummysliders[1][0]};
 
	t_runtime_object::Init("Gripper",robot,_p_control |_p_calc ,
			0,1,0,NULL,cb_pointers,NULL);
}



char t_gripper_demo::control_module(void)
{
    int i,L,R,r,rl,rr;
    float k,l,j;

    if (t_runtime_object::control_module() == false)
	return false;	    

    // check for collision 
    for (i=0; i<8; i++)
       if (Robot->Ired->Sensor[i]->Value > COLLISION_TH)
          break;
          
    if (i<8)
       { // COLLISION anywhere
//        t_PATH *last_pos;
        
//        last_pos = my_path->Add(&Robot->Loc.P);
//        last_pos->border = true;

        switch(i)
          {
           case 0:   // COLLISION is left
           case 1:
                   Robot->SetMotorSpeed(0,0);
                   Robot->Move(BACK_STEP);
//                   Robot->WaitStopped();
                   if (i==0)
                       Robot->Turn(-get_random(20,50));
                     else  
                       Robot->Turn(-get_random(70,110));
//                   Robot->WaitStopped();
           break;
           case 2:   // COLLISION is front
           case 3:
                   Robot->SetMotorSpeed(0,0);
                   Robot->WaitStopped();  
                   Robot->Move(2);
                   Robot->UpdateSensors();

//                   Display_IR(Robot);

                   r = 0;
                   k = 0.0;
                   l = 0.0;
                   for (i=0; i<8; i++)
                      {
                       if (Robot->Ired->Sensor[i]->Value > FREE_ROOM_TH) 
                          r++;
                       
                       k +=  Robot->Ired->Sensor[i]->Value;  
                       l +=  Robot->Ambl->Sensor[i]->Value;
                      }

                   if (r>2)
                      { // there must be a large object
//                        Robot->SetMotorSpeed(0,0);
                        Robot->Move(BACK_STEP);
                        Robot->WaitStopped();
                        Go_Away(Robot,160,10);
                        Robot->WaitStopped();
#ifdef DEBUG
                        fprintf(stderr,"wall detected k:%g\tj:%d\tl:%g\n",k,r,l);  
#endif                   
                        return true;
                      }   
#ifdef DEBUG
                      else
                        fprintf(stderr,"wall not detected k:%g\tj:%d\tl:%g\n",k,r,l);  
#endif                   
        	   // decide where to turn
        	   if ((Robot->Ired->Sensor[0]->Value > WALL_SEARCH_TH) ||
                       (Robot->Ired->Sensor[1]->Value > WALL_SEARCH_TH)    )
                           k = -75;
                       else if ((Robot->Ired->Sensor[4]->Value > WALL_SEARCH_TH) ||
                                (Robot->Ired->Sensor[5]->Value > WALL_SEARCH_TH)    )
                                k = 75;
                              else   
                                k = 60;
           
//                   Robot->WaitStopped();
                   // go back       
                   Robot->ReadWheelCounter(&L,&R);
                   Robot->Move(-GRIP_LEN);

                   if (Robot->CatchedObject())
                      { // we have already an object 
                       if (Robot->GrippedWidth()<200)
                          { // lay it down
//                           Robot->SetMotorSpeed(0,0);
                           Robot->Turn(k);    
//                           Robot->Turn(90);    
//                           Robot->WaitStopped();
                           Robot->SetArmPos(-30);
			   sleep(1);
//                           Robot->WaitStopped();
                           Robot->WaitStopped();

                           Robot->Release();
                           Robot->SetArmPos(45);
//                           Robot->WaitStopped();
                           Robot->Turn(-k);
                          }
                      }  
                   // put gripper in position
//                   Display_IR(Rob);
                   
                   Robot->SetArmPos(-42);
                   Robot->Release();
//                   Robot->WaitStopped();
                   // go forward slowly
                   Robot->Move(20);
                   Robot->SetMotorSpeed((double)GRIP_SPEED,(double)GRIP_SPEED);
                   do { 
//                        Display_IR(Rob);
                        Robot->ReadWheelCounter(&rl,&rr);
                      } while (!Robot->CatchedObject() && (rl<L) && (rr<R) && !((rl*rr)<0) );
                   // stop if touched,tired of turning or object disappeared
                   
                   // get it   
                   Robot->Move(10);
//                   Robot->WaitStopped();

   	           Robot->Grip();
                   Robot->WaitStopped();

                   // test the gripped object
                   if (Robot->ReadGripOhmMeter()>128)
                      {
                       Robot->Release();
                       Robot->Lift();
                      }
                   
                   if (Robot->CatchedObject())
                         {// try to lift it
                          Robot->WaitStopped();
                          Robot->Lift();
                          Robot->WaitStopped();
                          if (!Robot->CatchedObject())
                             { // the object was falling down
                              Go_Away(Robot,90,60);
                             } 
                          // we got the object   
                          Go_Away(Robot,11,10);   
                         }    
                       else
                         { // the object disapeared or was a wall
                          Robot->Lift();
                          Go_Away(Robot,170,10);
                         } 
//                   Robot->WaitStopped();      
           break;
           case 4:   // COLLISION is right
           case 5:
                   Robot->SetMotorSpeed(0,0);
                   Robot->Move(BACK_STEP);
//                   Robot->WaitStopped();
                   if (i==5)
                       Robot->Turn(get_random(20,50));
                     else  
                       Robot->Turn(get_random(70,110));
//                   Robot->WaitStopped();
//                   Robot->SetMotorSpeed(-2,2);
           break;
           case 6:   // COLLISION is rear
           case 7:
           break;
          }
//          Robot->WaitStopped();
       }
     else
       {
        // check all sensors   
        j = 0;
        for (i=0; i<8; i++)
           j += Robot->Ired->Sensor[i]->Value;

        // slow down eventually   
        if ((j/8) > FREE_ROOM_TH) 
            Robot->SetMotorSpeed((double)GRIP_SPEED,(double)GRIP_SPEED);
         else  
            Robot->SetMotorSpeed((double)RUN_SPEED,(double)RUN_SPEED);
       } 

	return true; 
}









/**/
/**/
/**/
/**/
/**/
/**/
/**/
/**/
/*****  End Of File  ****************************/
