#include "Dynamixel.h"
#define ID 91
bool mode=0;
task main()
{
  //-------------------------------------------------------------------------------------
  UseRS485(); // (1) Port S4 configured for RS485
  RS485Enable(); // (2) turn on RS485
  RS485Uart(HS_BAUD_57600, HS_MODE_DEFAULT); // (3) initialize UART 57600  HS_MODE_8_DATA
  Wait(100);
  //-------------------------------------------------------------------------------------
  TextOut (0, LCD_LINE1, "Wheel Mode");
  setMode(ID, WHEEL, 0, 0);
  //setMode(91, SERVO, 1, 1023);
  Wait(500);
  //setMode(91, SERVO, 0x001, 0xFFF);
  while(true)
  {
    if(ButtonPressed(BTNCENTER, FALSE)){               // If the center button is pushed, the program switches from               
         wheel(ID,1,0);  ClearScreen();                // wheel mode to servo mode and vice versa. 
         while(ButtonPressed(BTNCENTER, FALSE));
         if(mode==0)
         {
               setMode(91, SERVO, 1, 1023);// set servo mode
               Wait(500);
               mode=1;
         }
         else
         {
               setMode(ID, WHEEL, 0, 0);// set wheel mode
               Wait(500);
               mode=0;
         }

     }
     //-----------------------------------------------------------------------
     if(mode== 1) // for servo mode
     {
         TextOut (0, LCD_LINE1, "Servo Mode");
         if(ButtonPressed(BTNRIGHT, FALSE)){        // if Right Button is pressed
           while(ButtonPressed(BTNRIGHT, FALSE)); // Wait for Button is released
           servo(ID,50,300);// turn to 50 with speed 300
           Wait(1000);
           ClearScreen();
           TextOut (0, LCD_LINE3, "Move to 50");
          }else
         if(ButtonPressed(BTNLEFT, FALSE)){
           while(ButtonPressed(BTNLEFT, FALSE));
           servo(ID,1000,1000);  //turn to 1000 with speed 1000
           Wait(1000);
           ClearScreen();
           TextOut (0, LCD_LINE3, "Move to 1000");
          }
    }
    else // for wheel mode
    {
         TextOut (0, LCD_LINE1, "Wheel Mode");
        if(ButtonPressed(BTNRIGHT, FALSE)){        // if Right Button is pressed
           wheel(ID,RIGHT,800);  // turn right with speed 800
           ClearScreen();
           TextOut (0, LCD_LINE3, "RIGHT");
          }else
         if(ButtonPressed(BTNLEFT, FALSE)){
           wheel(ID,LEFT,500);  //turn left with speed 500
           ClearScreen();
           TextOut (0, LCD_LINE3, "LEFT");
         }
         else
         {
           wheel(ID,LEFT,0); //stop
           ClearScreen();
           TextOut (0, LCD_LINE3, "STOP");
         }
    }
   }
  RS485Disable();
}