smart_farm_rc_simple.ino

/**
 * Smart farm simple version
 * Control 2 motor with RC remote
 * Right horizontal stick control throttle 
 * Left vertical stick control steering
 * Created by : ModuleMore.com
 */
#include <Arduino.h>


/* -------------------------------------------------------------------------- */
/*                               System variable                              */
/* -------------------------------------------------------------------------- */
int _max_speed = 180;                                                           //Maximum duty cycle of motor [0-255]
int _remoteInput[6]={0};                                                        //Range : [1000, 2000]
float _throttle = 0.0f;                                                         //Range : [-1, 1]
float _steering = 0.0f;                                                         //Range : [-1, 1]
float _aux      = 0.0f;                                                         //Range : [-1, 1]

/* -------------------------------------------------------------------------- */
/*                               Receiver reader                              */
/* -------------------------------------------------------------------------- */
#include <PPMReader.h>
byte interrupt_pin = 3;                                                          //Digital pin 3(UNO)
byte channelAmount = 6;                                                          //Maximum channel valid in receiver
PPMReader ppm(interrupt_pin, channelAmount);                                     //Define class PPM reader


/* -------------------------------------------------------------------------- */
/*                                    Servo                                   */
/* -------------------------------------------------------------------------- */
#include <Servo.h>
Servo _servoOutput;                                                              //Define class Servo
int _servo_pin = 6;                                                              //Using pwm digital pin ~6

/* -------------------------------------------------------------------------- */
/*                              Motor Pin define                              */
/* -------------------------------------------------------------------------- */
//short PWM L and PWM R together on the bottom of PCB
#define M1_Pin_D2_L  12                                                          //rpwm
#define M1_Pin_D1_L  13                                                          //lpwm
#define M1_Pin_E_L   11                                                          //pwm enable

#define M2_Pin_D2_L  8                                                           //rpwm
#define M2_Pin_D1_L  9                                                           //lpwm
#define M2_Pin_E_L   5                                                           //pwm enable
  
void motor_cmd(int idx, bool rev, float pwm) {
  if(idx==0) {
    digitalWrite(M1_Pin_D1_L, !rev);
    digitalWrite(M1_Pin_D2_L, rev);
    analogWrite(M1_Pin_E_L, abs(pwm)*_max_speed);
  }else{
    digitalWrite(M2_Pin_D1_L, rev);
    digitalWrite(M2_Pin_D2_L, !rev);
    analogWrite(M2_Pin_E_L, abs(pwm)*_max_speed);
  } 
}

void motor_stop()
{
  digitalWrite(M1_Pin_D1_L, LOW);
  digitalWrite(M1_Pin_D2_L, LOW);
  analogWrite(M1_Pin_E_L, 0);

  digitalWrite(M2_Pin_D1_L, LOW);
  digitalWrite(M2_Pin_D2_L, LOW);
  analogWrite(M2_Pin_E_L, 0);
}

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

  /* -------------------------------------------------------------------------- */
  /*                              Initialize motor                              */
  /* -------------------------------------------------------------------------- */
  pinMode(M1_Pin_D1_L, OUTPUT);
  pinMode(M1_Pin_D2_L, OUTPUT);
  pinMode(M1_Pin_E_L, OUTPUT);

  pinMode(M2_Pin_D1_L, OUTPUT);
  pinMode(M2_Pin_D2_L, OUTPUT);
  pinMode(M2_Pin_E_L, OUTPUT);
  motor_stop();

  /* -------------------------------------------------------------------------- */
  /*                              Initialize servo                              */
  /* -------------------------------------------------------------------------- */
  _servoOutput.attach(_servo_pin); 

  delay(1000);
  Serial.println("Started!");
}

void loop()
{ 
    /* -------------------------------------------------------------------------- */
    /*                      Checking RC value from PPM format                     */
    /* -------------------------------------------------------------------------- */
    for (byte channel = 1; channel <= channelAmount; ++channel) {
        unsigned value = ppm.latestValidChannelValue(channel, 0);
        // Serial.print(String(value) + "\t");
        _remoteInput[channel-1] = constrain(value, 1000,2000);
    }
    // Serial.println();


    /* -------------------------------------------------------------------------- */
    /*                      Convert and feedforward to motor                      */
    /* -------------------------------------------------------------------------- */
    uint32_t cur_time = millis();
    static uint32_t last_cmd = 0;

    //This loop will run at 10Hz should be enought
    if( cur_time - last_cmd > 100) {
      last_cmd = cur_time;

      /* -------------------------------------------------------------------------- */
      /*                    Convert Remote Input to usable range                    */
      /* -------------------------------------------------------------------------- */
      
      _throttle = map(_remoteInput[1], 1000,2000,-10000,10000)/10000.0f;            // Convert scaling from [1000-2000] to [-1,1]
      _throttle = constrain(_throttle, 0, 1.0f);                                    // Clip to positive value (Shouldn't backward for now)
      _steering = map(_remoteInput[3], 1000,2000,-10000,10000)/10000.0f;                 
      _steering = constrain(_steering, -_throttle, _throttle);
      _aux      = map(_remoteInput[4], 1000,2000, -10000, 10000)/10000.0f;
      _aux      = constrain(_aux, 0, 1.0f);
   
      float l_out = _throttle + _steering;
      float r_out = _throttle - _steering;
      float norm = 1;
      
      if( abs(l_out) >1 || abs(r_out) > 1) {
        norm = sqrtf(l_out*l_out/2 + r_out*r_out/2); 
      }

      l_out=constrain(l_out/norm, 0.0f, 1.0f);
      r_out=constrain(r_out/norm, 0.0f, 1.0f);
      
 
      Serial.print(_throttle);Serial.print(", ");
      Serial.print(_steering);Serial.print(", ");
      Serial.print(_aux);Serial.print(", ");
 
      Serial.print(l_out);Serial.print(", ");
      Serial.print(r_out);Serial.print("\n");


      /* -------------------------------------------------------------------------- */
      /*                       Feed forward to motor and servo                      */
      /* -------------------------------------------------------------------------- */
      motor_cmd( 0, false, abs(l_out)>0.05 ? l_out : 0 );
      motor_cmd( 1, false, abs(r_out)>0.05 ? r_out : 0 );
      _servoOutput.write(180*_aux);


    }


    delay(20);
}