Motors controller

Motors controller is a subsystem made with:

  • Arduino Nano
  • CAN Controller
  • L292N H bridge
  • Motors

The Arduino board is connected to the CAN interface module and the H Bridge module.

As CAN messages arrive, it decides if they are relevant. Botler uses ID 10 for stopping the motors and ID 15 for movement.

The word sent with ID 15 is composed by 5 bytes. The first three are the direction, while the last two represent the percentage of speed requested.

Coding this ECU is not hard. This node just receives information from the CAN BUS. The message, if the ID is accepted, is then separated in two informations: direction and speed.

Speed will control the PWM signal sent to the H bridge, meanwhile direction will operate the signals to drive the bridge connections.

#include <SPI.h>
#include <mcp_can.h>
#include <mcp_can_dfs.h>

// H BRIDGE SETUP
#define ML_ENA  3
#define ML_IN1  4
#define ML_IN2  5
#define MR_ENA  6
#define MR_IN1  7
#define MR_IN2  8

// MOTORS SETUP
#define MINSPEED 15 
boolean ML_DIR = 0; 
boolean MR_DIR = 1; 
int motorSpeedA = 0;
int motorSpeedB = 0;
int action = 0;

// CAN BUS SETUP
long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
char msgString[128];                        
char onechar;
#define CAN0_INT 2                              
MCP_CAN CAN0(10);                               

// MOTORS CAN MESSAGE
//
// DDDSS
// DDD: DIRECTION (111 FORW, 100 LEFT, 001 RIGHT, 010 BACK)
// SS : % OF SPEED


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

  if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
  {
       Serial.println("MCP2515 Initialized Successfully!");
  }
  else
  {
       Serial.println("Error Initializing MCP2515...");
  }
  CAN0.setMode(MCP_NORMAL);                     
  pinMode(CAN0_INT, INPUT);                            
  
  pinMode(ML_ENA, OUTPUT);
  pinMode(ML_IN1, OUTPUT);
  pinMode(ML_IN2, OUTPUT);

  pinMode(MR_ENA, OUTPUT);
  pinMode(MR_IN1, OUTPUT);
  pinMode(MR_IN2, OUTPUT);
  
  digitalWrite(ML_IN1, !ML_DIR);
  digitalWrite(ML_IN2, ML_DIR);
  
  digitalWrite(MR_IN1, !MR_DIR);
  digitalWrite(MR_IN2, MR_DIR);
}

void can_command_dispatch(void)
{
     String incoming, data1, data2;
     int vel, dir;
     
     CAN0.readMsgBuf(&rxId, &len, rxBuf);      // Read data: len = data length, buf = data byte(s)
     incoming = rxBuf;

     if((rxId == 10)||(rxId == 15))
     {    
         data1 = incoming;
         data1.remove(3);
         incoming.remove(0,3);
         data2 = incoming;
         Serial.print("RECEIVED: ");
         Serial.print(data1);
         Serial.print(" and ");
         Serial.print(data2);
         Serial.print("%");
         Serial.println();
         dir = data1.toInt();
         vel = data2.toInt();      
    
         if((vel>=0)&&(vel<=100))
         {
           motorSpeedA = map(vel, 0, 100, 0, 255);
           motorSpeedB = map(vel, 0, 100, 0, 255);
           if(vel<MINSPEED)
           {
              motorSpeedA = 0;
              motorSpeedB = 0;
              dir = 0;
           }
         }
         action = dir;
     }
}

void left_fw (void)
{
   digitalWrite(ML_IN1, !ML_DIR);
   digitalWrite(ML_IN2, ML_DIR);
}

void left_bw (void)
{
   digitalWrite(ML_IN1, ML_DIR);
   digitalWrite(ML_IN2, !ML_DIR);
}

void right_fw (void)
{
   digitalWrite(MR_IN1, !MR_DIR);
   digitalWrite(MR_IN2, MR_DIR);
}

void right_bw (void)
{
   digitalWrite(MR_IN1, MR_DIR);
   digitalWrite(MR_IN2, !MR_DIR);
}

void controller (void)
{    
      switch(action)
      {
          case 111: // forward  
                    left_fw();
                    right_fw(); 
                    break; 
          case 100: // turn left
                    left_bw();
                    right_fw(); 
                    break;
          case 10:  // backwards
                    left_bw();
                    right_bw(); 
                    break; 
          case 1:   // turn right
                    left_fw();
                    right_bw(); 
                    break;  
          default : // stop
                    motorSpeedA = 0;
                    motorSpeedB = 0;
                    break; 
      }
}

void loop() 
{  
    if(!digitalRead(CAN0_INT))            // check if data coming
    {
       can_command_dispatch();
    }
    controller();
    analogWrite(ML_ENA, motorSpeedA); // Send PWM signal to motor A
    analogWrite(MR_ENA, motorSpeedB); // Send PWM signal to motor B
} 
Torna in alto