Arduino.ru

Какой прекрасный код нашел. Как я давно не видел профи.

Кайф.

Надо спинку почесать. :)

Размер скетча в двоичном коде: 9 956 байт (из 28 672 байт максимум)

https://yadi.sk/d/-X_0ME1mac8zY

// ------------- PROCESS AND INTERPRET SENSOR DATA -------------
void AvoidWalls() {
  // GET/SET URGE VALUES
  basicVelocity = 150; // Set above 0 to make it continuously move forward
  urgMotor_L    =   0;
  urgMotor_R    =   0;
  urgFatigue    =   0;

  // AVOID WALLS AT SIDE
  urgTurn_L  +=      maxPing*maxPing - ((maxPing-sonarRightVAL) * (maxPing-sonarRightVAL)); //inverse proportional to square of rightval
  urgTurn_R  +=      maxPing*maxPing - ((maxPing-sonarLeftVAL ) * (maxPing-sonarLeftVAL ));
  urgMotor_L -= 0.1*(maxPing*maxPing - ((maxPing-sonarRightVAL) * (maxPing-sonarRightVAL)));
  urgMotor_R -= 0.1*(maxPing*maxPing - ((maxPing-sonarLeftVAL ) * (maxPing-sonarLeftVAL )));
  
  // AVOID OBJECTS IN FRONT
  urgMotor_L += maxPing*maxPing - 0.5*((maxPing-sonarFrontLeftVAL ) * (maxPing-sonarFrontLeftVAL )) - ((maxPing-sonarFrontRightVAL) * (maxPing-sonarFrontRightVAL));
  urgMotor_R += maxPing*maxPing - 0.5*((maxPing-sonarFrontRightVAL) * (maxPing-sonarFrontRightVAL)) - ((maxPing-sonarFrontLeftVAL ) * (maxPing-sonarFrontLeftVAL ));
  
  // SCALE URGES TO PWM output values (255)
  urgTurn_L  = 255 - map(urgTurn_L , 0, 1.8*maxPing*maxPing, -255, 255);  // Scale to within PWM output limits
  urgTurn_R  = 255 - map(urgTurn_R , 0, 1.8*maxPing*maxPing, -255, 255);
  urgMotor_L =       map(urgMotor_L, 0, 1.8*maxPing*maxPing, -255, 255);  // Scale to within PWM output limits
  urgMotor_R =       map(urgMotor_R, 0, 1.8*maxPing*maxPing, -255, 255);

 // SET MOTOR SPEED 
  newMotorSPD_L = basicVelocity + urgMotor_L + (urgTurn_R/4) - (urgTurn_L/2) + 60; 
  newMotorSPD_R = basicVelocity + urgMotor_R + (urgTurn_L/4) - (urgTurn_R/2) + 60; 
  
  // Clip to 255/-255 (negative value means reverse direction)
  if (newMotorSPD_L >  255) newMotorSPD_L =  255;
  if (newMotorSPD_L < -255) newMotorSPD_L = -255;  
  if (newMotorSPD_R >  255) newMotorSPD_R =  255;
  if (newMotorSPD_R < -255) newMotorSPD_R = -255;
}