Arduino.ru

Добрый день, коллеги.

Написал скейтч для управления двумя сервами и моточиком впридачу. Подключил джойстик - все летает (даже скорость ограничил - видно в коде), как только дело касается приема данных через сеть - двигается все ужаснейшими рывками. Хотя, скорость должна быть большая. Помогите пожалуйста понять в чем косяк у меня.

#include <SPI.h>        
#include <Ethernet.h>
#include <Servo.h> 

byte mac[] = { 
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED}; 
IPAddress ip(192,168,1,190);

// local port to listen on
const int LOCAL_PORT = 9082;      

// buffer for debug output
char buffer[64];

EthernetServer server(LOCAL_PORT);
EthernetClient client;

// PIN CONFIG
#define X_AXIS A0
#define Y_AXIS A1
#define Z_SW 7
#define M_FORWARD 5
#define M_BACKWARD 4

#define D_STOP 0;
#define D_UP 1;
#define D_DOWN 2;
#define D_LEFT 3;
#define D_RIGHT 4;

enum cmd { 
  UNKNOWN=0x00, MOVE_N=0x01, 
  MOVE_NE=0x02, MOVE_E=0x03, 
  MOVE_SE=0x04, MOVE_S=0x05, 
  MOVE_SW=0x06, MOVE_W=0x07, 
  MOVE_NW=0x08, PULL=0x09,
  RELEASE=0x0A, SERVICE=0x0B,
  STATUS=0x0C};

byte act = 0x00;

int POS_PLATFORM = 90;
int POS_CONSOLE = 90;

Servo S_PLATFORM;
Servo S_CONSOLE;

int POS_PLATFORM_MAX = 180;
int POS_PLATFORM_MIN = 0;
int POS_CONSOLE_MAX = 140;
int POS_CONSOLE_MIN = 30;
int POS_STEP = 5;
int X, Y;

void setup() {

  Serial.begin(9600);
  Ethernet.begin(mac,ip);
  server.begin();
  client.setTimeout(10);
  Serial.println("INIT READY");

  pinMode(Z_SW, INPUT);
  pinMode(M_FORWARD, OUTPUT);
  pinMode(M_BACKWARD, OUTPUT);

  digitalWrite(M_FORWARD,0);
  digitalWrite(M_BACKWARD,0);    

  S_PLATFORM.attach(2);
  S_CONSOLE.attach(3);
}

int treatValue(int data) {
  return (data * 9 / 1024);
}

void getJoyData(int &POS_PLATFORM, int &POS_CONSOLE) 
{
  int cX = treatValue(analogRead(X_AXIS));
  int cY = treatValue(analogRead(Y_AXIS));

  if(cY < 5 ) {   
    POS_CONSOLE = POS_CONSOLE - POS_STEP;
  }
  if(cY > 5 ) {   
    POS_CONSOLE = POS_CONSOLE + POS_STEP;    
  }

  if(cX < 5 ) {   
    POS_PLATFORM = POS_PLATFORM - POS_STEP;
  }
  if(cX > 5 ) {   
    POS_PLATFORM = POS_PLATFORM + POS_STEP;
  }

  if(POS_PLATFORM <= POS_PLATFORM_MIN ) { 
    POS_PLATFORM = POS_PLATFORM_MIN; 
  }
  if(POS_PLATFORM >= POS_PLATFORM_MAX ) { 
    POS_PLATFORM = POS_PLATFORM_MAX; 
  }

  if(POS_CONSOLE <= POS_CONSOLE_MIN ) { 
    POS_CONSOLE = POS_CONSOLE_MIN; 
  }
  if(POS_CONSOLE >= POS_CONSOLE_MAX ) { 
    POS_CONSOLE = POS_CONSOLE_MAX; 
  }

  S_PLATFORM.write(POS_PLATFORM);
  S_CONSOLE.write(POS_CONSOLE); 

  delay(15); 
}


void loop()
{
  getJoyData(*&POS_PLATFORM, *&POS_CONSOLE);

  // listen for incoming clients
  client = server.available();
  if(client){
    //    Serial.println("--CLIENT");
    while(client.connected()){
      //Serial.println("--CONNECTED");
      while(client.available()) {

        char raw_len[1];
        client.readBytes(raw_len, 1);
        byte len = *(byte*)raw_len;

        Serial.print(len);
        Serial.println(" :CMD COUNT");

        byte raw_act[len];
        //client.readBytes(raw_act, len);

        for(int i=0; i<len; i++)
        {
          if(i>=3){ client.flush(); }
          
          //byte act = *(byte*)raw_act[i];
          byte act = client.read();
          int action = readCMD(act);
          Serial.println(action);
          switch((cmd)action)
          {
          case MOVE_N:
            {
              MoveConsoleUP();
              //Serial.println("MOVE N");
              break;
            } 
          case MOVE_NE:
            {
              MoveConsoleUP();
              MovePlatformRIGHT();
              //Serial.println("MOVE NE");
              break;
            } 
          case MOVE_E:
            {
              MovePlatformRIGHT();
              //Serial.println("MOVE E");
              break;
            } 
          case MOVE_SE:
            {
              MoveConsoleDOWN();
              MovePlatformRIGHT();
              //Serial.println("MOVE SE");
              break;
            } 
          case MOVE_S:
            {
              MoveConsoleDOWN();
              //Serial.println("MOVE S");
              break;
            } 
          case MOVE_SW:
            {
              MoveConsoleDOWN();
              MovePlatformLEFT();              
              //Serial.println("MOVE SW");
              break;
            }             

          case MOVE_W:
            {
              MovePlatformLEFT();              
              //Serial.println("MOVE W");
              break;
            } 
          case MOVE_NW:
            {
              MoveConsoleUP();
              MovePlatformLEFT();              

              //Serial.println("MOVE NW");
              break;
            } 
          case PULL:
            {
              Serial.println("PULL");
              Pull();
              break;
            }                         

          case RELEASE:
            {
              Release();
              Serial.println("RELEASE");
              break;
            } 
          case SERVICE:
            {
              Serial.println("SERVICE");
              break;
            }                 
          case STATUS:
            {
              Serial.println("STATUS");
              break;
            }                 
          default:
            {
              Serial.println("ACT UNKNOWN - DO NOTHING");
              break;
            }
          }
          Serial.print("POS CONSOLE: ");
          Serial.println(POS_CONSOLE);

          Serial.print("POS PLATFORM: ");
          Serial.println(POS_PLATFORM);

            S_PLATFORM.write(POS_PLATFORM);
          S_CONSOLE.write(POS_CONSOLE);  
        }

      }
    }
    delay(1);
    client.stop();
    Serial.println("--STOP");
  }
}

// --------- commands ---------------

int readCMD(byte c)
{
  cmd val;
  if(c >= 0 || c <= 12) { 
    val = (cmd)c;  
  } 
  else { 
    val = UNKNOWN; 
  }
  return val;
}


// ---------- ACTIONS -----------------

void MoveConsoleUP()
{
  Serial.print("CON UP");
  POS_CONSOLE = POS_CONSOLE + POS_STEP;
  if(POS_CONSOLE >= POS_CONSOLE_MAX ) { 
    POS_CONSOLE = POS_CONSOLE_MAX; 
  }  
}

void MoveConsoleDOWN()
{
  Serial.print("CON DOWN");
  POS_CONSOLE = POS_CONSOLE - POS_STEP;
  if(POS_CONSOLE <= POS_CONSOLE_MIN ) { 
    POS_CONSOLE = POS_CONSOLE_MIN; 
  }
}

void MovePlatformRIGHT()
{
  Serial.print("PL RIGHT");  
  POS_PLATFORM = POS_PLATFORM - POS_STEP;
  if(POS_PLATFORM >= POS_PLATFORM_MAX ) { 
    POS_PLATFORM = POS_PLATFORM_MAX; 
  }
}

void MovePlatformLEFT()
{
  Serial.print("PL LEFT");  
  POS_PLATFORM = POS_PLATFORM + POS_STEP;  
  if(POS_PLATFORM <= POS_PLATFORM_MIN ) { 
    POS_PLATFORM = POS_PLATFORM_MIN; 
  }  
}

void Pull()
{
  digitalWrite(M_BACKWARD,1);
}  

void Release()
{
  digitalWrite(M_FORWARD,1);
}

void StopMotor()
{
  digitalWrite(M_FORWARD,0);
  digitalWrite(M_BACKWARD,0);    
}

Либы старнадтные, те что входят в комплект ардуино IDE. Ничего экстраординароного и все просто. Мой ли косяк аль что в коде эзернет либы, я вчера бегло погуглил - ругаются на нее сильно.

Буду рад любой помощи и замечаниям даже по качеству написанию кода.
Благодарю и доброго дня.