roboforum.ru

' Get turret control variables
flt_base = 10000

rate_v  = GetVariable("RATE_V")/flt_base
turret_v  = GetVariable("TURRET_V")/flt_base
turret_Sv = GetVariable("TURRET_V_SPEED")

rate_h  = GetVariable("RATE_H")/flt_base
turret_h  = GetVariable("TURRET_H")/flt_base
turret_Sh = GetVariable("TURRET_H_SPEED")
turret_f  = GetVariable("TURRET_FIRE")
step_counter = GetVariable("STEP_COUNTER")

dX = 0
dY = 0

status = ""
turret_v_initial = -80

nvObjectsTotal = GetVariable("NV_OBJECTS_TOTAL")

if nvObjectsTotal>0 then   ' If any object was found

   ' Get image size
   img_w = GetVariable("IMAGE_WIDTH")
   img_h = GetVariable("IMAGE_HEIGHT")
   
   ' Get array variables of recognized objects
   nvArrObjRectX = GetArrayVariable("NV_ARR_OBJ_RECT_X")
   nvArrObjRectY = GetArrayVariable("NV_ARR_OBJ_RECT_Y")
   nvArrObjRectW = GetArrayVariable("NV_ARR_OBJ_RECT_W")
   nvArrObjRectH = GetArrayVariable("NV_ARR_OBJ_RECT_H")

   ' Get center coordinates of first object from array
   obj_x = nvArrObjRectX(0) + nvArrObjRectW(0)/2
   obj_y = nvArrObjRectY(0) - nvArrObjRectH(0)/2
   
   ' Get difference between object and screen centers
   dX = img_w/2 - obj_x
   dY = img_h/2 - obj_y
   
   dXr = 1 - abs(dX*4/img_w)
   if dXr < 0 then dXr = 0
   
   dYr = 1 - abs(dY*4/img_h)
   if dYr < 0 then dYr = 0
   
   turret_min = -100
   turret_max = 100
   reaction   = 7
   speed_min  = 1
   speed_max  = 100
   filtering  = 0.7
   decay      = 0.1
   threshold  = round(img_w*0.03)

   sRateH = exp(-dXr*reaction)
   sRateV = exp(-dYr*reaction)
   
   rate_h = rate_h + (sRateH - rate_h)*filtering
   rate_v = rate_v + (sRateV - rate_v)*filtering
   
   turret_Sh = round(speed_min + rate_h*(speed_max - speed_min))
   turret_Sv = round(speed_min + rate_v*(speed_max - speed_min))
   
   delta_h = (img_w/8)*rate_h
   delta_v = (img_h/8)*rate_v

  if step_counter =< 0 then
      step_counter = round(exp(-(dXr*dYr)*reaction*0.7)*15)
      
      if dX > threshold then
         ' The object is at left side
         turret_h = turret_h - delta_h
      
         if turret_h < turret_min then turret_h = turret_min
      end if

      if dX < -threshold then
         ' The object is at right side
         turret_h = turret_h + delta_h
      
         if turret_h > turret_max then turret_h = turret_max
      end if
   
      if dY > threshold then
         ' The object is at the bottom
         turret_v = turret_v - delta_v
      
         if turret_v < turret_min then turret_v = turret_min
      end if
   
      if dY < -threshold then
         ' The object is at the top
         turret_v = turret_v + delta_v
      
         if turret_v > turret_max then turret_v = turret_max
      end if
   else
      step_counter = step_counter - 1
   end if
      
   ' Is the target locked?
   if dX < threshold and dX > -threshold and dY < threshold and dY > -threshold then
      status = "Target is locked"
      turret_f = 1
   else
      status = "Tracking"
      turret_f = 0
   end if
else
   ' Back to the center if object is lost
   if turret_h > 0 then turret_h = turret_h - 1
   if turret_h < 0 then turret_h = turret_h + 1
   if turret_v > turret_v_initial then turret_v = turret_v - 1
   if turret_v < turret_v_initial then turret_v = turret_v + 1
   
   turret_Sh = speed_min
   turret_Sv = speed_min
   
   rate_h = rate_h - rate_h*decay
   rate_v = rate_v - rate_v*decay
   
   turret_f = 0
end if

' Set turret control variables
SetVariable "RATE_V", rate_v*flt_base
SetVariable "TURRET_V", turret_v*flt_base
SetVariable "TURRET_V_CONTROL", round(turret_v)
SetVariable "TURRET_V_SPEED", turret_Sv
SetVariable "RATE_H", rate_h*flt_base
SetVariable "TURRET_H", turret_h*flt_base
SetVariable "TURRET_H_CONTROL", round(turret_h)
SetVariable "TURRET_H_SPEED", turret_Sh
SetVariable "TURRET_FIRE", turret_f
SetVariable "STEP_COUNTER", step_counter
SetVariable "DELTA_X", dX
SetVariable "DELTA_Y", dY
SetVariable "TURRET_STATUS", status

#include <Servo.h>

Servo servos[12];
boolean pinModes[14];

unsigned int crc;
unsigned int command;
unsigned int channel;
unsigned int value;
unsigned int valueLow;
unsigned int valueHigh;
unsigned int streamDigital;
unsigned int streamAnalog;
unsigned int lastDigital;
unsigned int lastAnalog[8];
unsigned int heartBeat=0;
unsigned int defaultServo[14];

#define ARDUINO_GET_ID 0
#define ARDUINO_SET_SERVO 1
#define ARDUINO_SET_DIGITAL_STREAM 2
#define ARDUINO_SET_DIGITAL_HIGH 3
#define ARDUINO_SET_DIGITAL_LOW 4
#define ARDUINO_SET_ANALOG_STREAM 5
#define ARDUINO_DIGITAL_STREAM 6
#define ARDUINO_ANALOG_STREAM 7
#define ARDUINO_SET_ANALOG 8
#define ARDUINO_SET_SERVO_DEFAULT 9
#define ARDUINO_HEARTBEAT 10

void initialize()
{
  int i;
  for (i=2;i<14;i++) pinModes[i]=-1;
  streamDigital=0;
  streamAnalog=0;
  lastDigital=-1;
  for (i=0;i<8;i++) lastAnalog[i]=-1;
  for (i=0;i<12;i++) defaultServo[i]=1500;
}

void setup()
{
  Serial.begin(115200);

  initialize();
}

void writePacket()
{
  unsigned char buffer[2]; 
  buffer[0]=command|128;
  buffer[1]=channel;
  Serial.write(buffer, 2);
}

void writeValuePacket(int value)
{
  unsigned char buffer[5]; 
 
  buffer[0]=command|128;
  buffer[1]=channel;
  buffer[2]=value&127;
  buffer[3]=(value>>7)&127;
  buffer[4]=(buffer[0]^buffer[1]^buffer[2]^buffer[3])&127;
 
  Serial.write(buffer, 5);
}

void readPacket()
{
  // get header byte
  // 128 (bit 8) flag indicates a new command packet .. that
  // means the value bytes can never have 128 set!
  // next byte is the command 0-8
  // next byte is the channel 0-16

  do
  {
    while (Serial.available() <= 0) continue;
    command = Serial.read();
  }
  while ((command&128)==0);
 
  command^=128;

  while (Serial.available() <= 0) continue;
  channel = Serial.read();
}

int readValuePacket()
{
  unsigned int valueLow;
  unsigned int valueHigh;
 
  // wait for value low byte   
  while (Serial.available() <= 0) continue;
  valueLow = Serial.read();
  if (valueLow&128) return 0;

  // wait for value high byte   
  while (Serial.available() <= 0) continue;
  valueHigh = Serial.read();
  if (valueHigh&128) return 0;
   
  // wait for crc byte   
  while (Serial.available() <= 0) continue;
  crc = Serial.read();
  if (crc&128) return 0;
 
  if (crc!=(((128|command)^channel^valueLow^valueHigh)&127)) return 0;

  value = valueLow|(valueHigh<<7);
 
  return 1;
}

void loop()
{
  while (Serial.available()>0)
  {
    readPacket();
   
    heartBeat=0;

    switch (command)
    {
      // init
      case  ARDUINO_GET_ID:
        initialize();
        Serial.print("ARDU");
      break;
      // servo
      case  ARDUINO_SET_SERVO:
        if ((channel>=3)&&(channel<=11))
        {
          if (readValuePacket())
          {
            if (pinModes[channel]!=1)
            {
              servos[channel].attach(channel);
              pinModes[channel]=1;
            }
            servos[channel].writeMicroseconds(value);
            writeValuePacket(value);
          }
        }
      break;
      //digital stream
      case  ARDUINO_SET_DIGITAL_STREAM:
        if (readValuePacket())
        {
          streamDigital = value;
          writeValuePacket(value);
          lastDigital=-1;
        }
      break;
      //set digital high
      case  ARDUINO_SET_DIGITAL_HIGH:
        if ((channel>=2)&&(channel<14))
        {
          if (pinModes[channel]!=2)
          {
            if (pinModes[channel]==1)
              servos[channel].detach();

            pinMode(channel, OUTPUT);
            pinModes[channel]=2;
            if (streamDigital&(1<<channel))
              streamDigital^=1<<channel;
          }
         
          digitalWrite(channel, HIGH);
          writePacket();
        }
      break;
      //set digital low
      case  ARDUINO_SET_DIGITAL_LOW:
        if ((channel>=2)&&(channel<14))
        {
          if (pinModes[channel]!=2)
          {
            if (pinModes[channel]==1)
              servos[channel].detach();
             
            pinMode(channel, OUTPUT);
            pinModes[channel]=2;
           
            if (streamDigital&(1<<channel))
              streamDigital^=1<<channel;
          }
          digitalWrite(channel, LOW);
          writePacket();
        }
      break;
      //analog stream
      case  ARDUINO_SET_ANALOG_STREAM:
        if (readValuePacket())
        {
          streamAnalog = value;
          writeValuePacket(value);
          for (channel=0;channel<8;channel++) lastAnalog[channel]=-1;
        }
      break;
      case  ARDUINO_SET_ANALOG:
        if (readValuePacket())
        {
               if ((channel>=3)&&(channel<=11))
               {
                  if (pinModes[channel]!=2)
                  {
                     if (pinModes[channel]==1)
                        servos[channel].detach();

                     pinMode(channel, OUTPUT);
                     pinModes[channel]=2;
                  }
                  analogWrite(channel, value);
                  writeValuePacket(value);
               }
        }
      break;
      case  ARDUINO_SET_SERVO_DEFAULT:
        if ((channel>=3)&&(channel<=11))
        {
          if (readValuePacket())
          {
            defaultServo[channel] = value;
                  writeValuePacket(value);
          }
        }
      break;
      case ARDUINO_HEARTBEAT:
      break;
    }
  }
 
  if ((heartBeat++)>25000)
  {
    int i;
    for (i=3;i<12;i++)
    {
      if (pinModes[i]==1)
        servos[i].writeMicroseconds(defaultServo[i]);
      else
      if (pinModes[i]==2)
        analogWrite(i, 0);
    }
  }

  for (channel=0;channel<8;channel++)
  {
    if (streamAnalog&(1<<channel))
    {
      value = analogRead(channel);
      // only send value if it has changed
      if (value!=lastAnalog[channel])
      {
        command = ARDUINO_ANALOG_STREAM;
        writeValuePacket(value);
       
        lastAnalog[channel]=value;
      }
    }
  }

  if (streamDigital)
  {
    value=0;
    for (channel=2;channel<14;channel++)
    {
      if (streamDigital&(1<<channel))
      {
        if (pinModes[channel]!=3)
        {
          if (pinModes[channel]==1)
            servos[channel].detach();
           
          pinMode(channel, INPUT);
          pinModes[channel]=3;
          // pullup
          digitalWrite(channel, HIGH);
        }
       
        value |= digitalRead(channel)<<channel;
      }
    }

    // only send value if it has changed
    if (value!=lastDigital)
    {   
      command = ARDUINO_DIGITAL_STREAM;
      writeValuePacket(value);

      lastDigital=value;
    }
  }
}

reaction   = 7

step_counter = round(exp(-(dXr*dYr)*reaction*0.7)*15)

   ' Back to the center if object is lost
   if turret_h > 0 then turret_h = turret_h - 1
   if turret_h < 0 then turret_h = turret_h + 1
   if turret_v > turret_v_initial then turret_v = turret_v - 1
   if turret_v < turret_v_initial then turret_v = turret_v + 1