#include "Dynamixel.h"
#define ID 91
#define ID1 40
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);Wait(10);
  setMode(ID1, 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 mode to servo mode and vice versa.
         wheel(ID,1,0);  ClearScreen();
         while(ButtonPressed(BTNCENTER, FALSE));
         mode++;
         if(mode==3) mode=0;
     }
     //-----------------------------------------------------------------------
     if(mode== 0) // for servo mode
     {
         TextOut (0, LCD_LINE1, "Wheel Mode 0");
        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");
         }
    }
    else if(mode== 1) // for servo mode
     {
         TextOut (0, LCD_LINE1, "Wheel Mode 1");
        if(ButtonPressed(BTNRIGHT, FALSE)){        // if Right Button is pressed
           wheel(ID1,RIGHT,800);  // turn right with speed 800
           ClearScreen();
           TextOut (0, LCD_LINE3, "RIGHT");
          }else
         if(ButtonPressed(BTNLEFT, FALSE)){
           wheel(ID1,LEFT,500);  //turn left with speed 500
           ClearScreen();
           TextOut (0, LCD_LINE3, "LEFT");
         }
         else
         {
           wheel(ID1,LEFT,0); //stop
           ClearScreen();
           TextOut (0, LCD_LINE3, "STOP");
         }
    }
    else // for wheel mode
    {
        TextOut (0, LCD_LINE1, "Wheel Mode 2");
        if(ButtonPressed(BTNRIGHT, FALSE)){        // if Right Button is pressed
           wheel(ID,RIGHT,1000);  // turn right with speed 800
           wheel(ID1,RIGHT,300);
           ClearScreen();
           TextOut (0, LCD_LINE3, "RIGHT");
          }else
         if(ButtonPressed(BTNLEFT, FALSE)){
           wheel(ID,LEFT,300);  //turn left with speed 500
           wheel(ID1,LEFT,1000);  //turn left with speed 500
           ClearScreen();
           TextOut (0, LCD_LINE3, "LEFT");
         }
         else
         {
           wheel(ID,LEFT,0); //stop
           wheel(ID1,LEFT,0); //stop
           ClearScreen();
           TextOut (0, LCD_LINE3, "STOP");
         }
    }
   }
  RS485Disable();
}