roboforum.ru

char read_next_byte()
{
   while(serial_get_received_bytes() == read_index)
   {
   }
   char ret = buffer[read_index];
   read_index ++;
   if(read_index >= 100)
      read_index = 0;
   return ret;
}

unsigned int read_next_short()
{
   unsigned int value;
   value = (unsigned int)(read_next_byte() << 8);
   value += (unsigned int)(read_next_byte());
   return value;
}

int main()
{
   delay_ms(2000);
   servos_start((unsigned char[]) {IO_D2, IO_D4}, 2);

   set_servo_speed(0, 0);
   set_servo_speed(1, 0);
   set_servo_target(0, 1500);
   set_servo_target(1, 1500);
   
   serial_set_baud_rate(9600);
   serial_receive_ring(buffer, 100);
   send_signature();

   while(1)
   {
      // wait for a command
      char command = read_next_byte();

      // The list of commands is below: add your own simply by
      // choosing a command byte and introducing another case
      // statement.
      switch(command)
      {
      case (char)0x00:
         // silent error - probable master resetting
         break;

      case (char)0x81:
         send_signature();
         break;
      case (char)0xB0:
         send_trimpot();
         break;
      case (char)0xB1:
         send_battery_millivolts();
         break;

      case (char)0xC1:
         m1_forward();
         break;
      case (char)0xC2:
         m1_backward();
         break;
      case (char)0xC5:
         m2_forward();
         break;
      case (char)0xC6:
         m2_backward();
         break;

      case (char)0xC7:
         set_servo_target(0, read_next_short());
         break;
      case (char)0xC8:
         set_servo_target(1, read_next_short());
         break;
      case (char)0xC9:
         set_servo_speed(0, read_next_short());
         break;
      case (char)0xCA:
         set_servo_speed(1, read_next_short());
         break;

      default:
         continue; // bad command
      }
   }
}

      private void panSlider_ValueChanged(object sender, EventArgs e)
      {
         byte[] cmds = new byte[3];
         cmds[0] = (byte)0xC7;
         cmds[1] = (byte)(panSlider.Value >> 8);
         cmds[2] = (byte)(panSlider.Value);

         serialPort.Write(cmds, 0, 3);
         label1.Text = panSlider.Value.ToString();
      }

      private void tiltSlider_ValueChanged(object sender, EventArgs e)
      {
         byte[] cmds = new byte[3];
         cmds[0] = (byte)0xC8;
         cmds[1] = (byte)(tiltSlider.Value >> 8);
         cmds[2] = (byte)(tiltSlider.Value);

         serialPort.Write(cmds, 0, 3);
         label2.Text = tiltSlider.Value.ToString();
      }