ccc

global key
InitKeyboard();
state = 0;
timerVal = 0;
turnCount = 1;
rtSpeed = 0.78;
control = false;
speed = 45;
lastDist = 0;
incDec = 0;

% Placeholder for timer.
while 1
    pause(0.1);


     if(key == 'q')
        brick.StopAllMotors();
        break;
     end

     if(key == 'c')
        brick.MoveMotorAngleRel('AD', 0, 0, 'Brake');
        brick.MoveMotorAngleRel('AD', 0, 0, 'Coast');
        control = true;
     end

     if control == true
       if(key == '1') % white claw B
        brick.MoveMotorAngleRel('B', -40, 30, 'Coast');
       end


       if(key == '3') % other claw C
         brick.MoveMotorAngleRel('C', -40, 30, 'Coast');
       end

        if(key == '2') % white claw B
          brick.MoveMotorAngleRel('B', 40, 30, 'Coast');
       end


       if(key == '4') % other claw C
        brick.MoveMotorAngleRel('C', 40, 30, 'Coast');
       end

       if(key == 'x') % end control
           control = false;
       end


       switch key
       case 'uparrow'
            brick.MoveMotor('AD', speed);

       case 'downarrow'
            brick.MoveMotor('AD', -speed-15);

       case 'leftarrow'
            brick.MoveMotor('A', -speed);
            brick.MoveMotor('D', speed);


       case 'rightarrow'
            brick.MoveMotor('A', speed);
            brick.MoveMotor('D', -speed);
       case 0
           brick.MoveMotorAngleRel('AD', 0, 0, 'Brake');
           brick.MoveMotorAngleRel('AD', 0, 0, 'Coast');
       end



     else

    switch state
        case 0
            % Move Forward until Bumped
            curr = brick.UltrasonicDist(1);
           if  curr < lastDist
               incDec = 7.5;
           else
               incDec = -7.5;
           end

            brick.ResetMotorAngle('AD');
            brick.MoveMotor('A', 85);
            brick.MoveMotor('D', 85+(incDec));

            % color
       %     color = brick.ColorColor(3);
        %    if color == 4 % yellow
         %       brick.MoveMotorAngleRel('AD', 0, 0, 'Brake');
          %      brick.MoveMotorAngleRel('AD', 0, 0, 'Coast');
         %       pause(3);
        %    end


            % Output Action
            if brick.TouchPressed(4)
                % State Transition
                brick.MoveMotor('AD', 0);
                state = 1;
                timerVal = tic;
                % start timer.
            end

            lastDist =  brick.UltrasonicDist(1);
            if lastDist > 60.0
                state = 3;
                timerVal = tic;
            end


        case 1
            %Bumped, Reverse for a time.
            % Output Action
            brick.MoveMotor('AD', -100);
            if(toc(timerVal) > 0.50) % 0.57
                % State Transition after 1 seconds
                state = 2;
                timerVal = tic;
                % Reset Timer
            end
        case 2
            % Done Reversing, turn left
            brick.MoveMotor('A', -50);
            brick.MoveMotor('D', 50);
            if(toc(timerVal) > 0.8) % 0.42
                % State Transition after 0.5 seconds
                state = 0;
            end

        case 3

            % Done detecting, turn right
            brick.MoveMotor('A', 50);
            brick.MoveMotor('D', -50);
          %  if turnCount == 3
          %      rtSpeed = 0.55;
           % end
            if(toc(timerVal) > 0.8)
                % State Transition after 0.5 seconds
                turnCount = turnCount +1;
                state = 4;
            end
        case 4
            % move for few seconds after turning right
            brick.MoveMotor('AD', 85);
            if(toc(timerVal) > 2)
                % State Transition after 0.5 seconds
                state = 0;
            end


    end

end

end
CloseKeyboard();