Arm controller

The arm controller is a simple ECU with:

  • Arduino Nano
  • CAN BUS interface
  • Adafruit 16x12bit PWM controller

On the rover there will be a robotic arm with 6 degrees of freedom (6 servo) and one gripper (+ 1 servo).

It will require multiple CAN messages to deliver all the informations needed.

The code works with the following IDs:

  • 16: ImmmMMM (ID, minimum value and maximum value)
  • 17: IRRRTTT (ID, rest/safe position and target value)
  • 18: VVV (speed of movement)
  • 38: ImmmMMM (ID, minimum value and maximum value)

Messages 16 and 38 are identical, but the ID will produce different results, as described later.

In order to move a servo we need to know the range it can use. In the web interface there is a setup page where it is possible to store those informations. Another parameter is the safe or rest position. Target value is the final desired position of our servo and the speed of movement specify its rate.

Message 38 requires a servo sweep, giving the servo ID and the range of movement. This allows a fast, easy and visual setup.

#include "FaBoPWM_PCA9685.h"
#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
char msgString[128];                        // Array to store serial string
char onechar;
#define CAN0_INT 3                              // Set INT to pin 2
MCP_CAN CAN0(10);                               // Set CS to pin 10

FaBoPWM faboPWM;

#define CLAW 0
#define S6 1
#define S5 2
#define S4 3
#define S3 4
#define S2 5
#define S1 6

const int servo_nr = 8;
int pnow[8] = {100,100,100,100,100,100,100,100};
int pdes[8] = {100,100,100,100,100,100,100,100}; 
int pspe[8] = {1,1,1,1,1,1,1,1};
                                 
void setup() 
{
 
  Serial.begin(57600); 
        
  if(faboPWM.begin()) 
  {
    Serial.println("Find PCA9685");    
    faboPWM.init(100);
  }
  faboPWM.set_hz(50);
  
  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);                     // Set operation mode to normal so the MCP2515 sends acks to received data.

  pinMode(CAN0_INT, INPUT);                            // Configuring pin for /INT input
}

unsigned long int hexToDec(String hexString) 
{
  unsigned long int decValue = 0;
  long int nextInt;
  
  for (int i = 0; i < hexString.length(); i++) {
    
    nextInt = int(hexString.charAt(i));
    if (nextInt >= 48 && nextInt <= 57) nextInt = map(nextInt, 48, 57, 0, 9);
    if (nextInt >= 65 && nextInt <= 70) nextInt = map(nextInt, 65, 70, 10, 15);
    if (nextInt >= 97 && nextInt <= 102) nextInt = map(nextInt, 97, 102, 10, 15);
    nextInt = constrain(nextInt, 0, 15);
    
    decValue = (decValue * 16) + nextInt;
  }
  
  return decValue;
}

void move_servo(void)
{
   int cur, des, spe, dist, fut;
   int dir = 1;
   for(int i=0;i<servo_nr;i++)
   {
        cur = pnow[i];
        des = pdes[i];
        spe = pspe[i];
        if (cur != des)   // if current position is not the desired position
        {
           if(cur<des)    // if current position is lower than the desired one
           {
              dir = 1;   // direction is UP
              dist = des - cur;  // distance remaining between current and desired
              if(dist<spe) // if distance is lower than speed
              {
                 spe = dist;  // speed is equalized to dist for this step
              }
           }
           else
           {
              dir = -1; // direction is DOWN
              dist = cur - des; // distance remaining between current and desired
              if(dist<spe) // if distance is lower than speed
              {
                 spe = dist; // speed is equalized to dist for this step
              }
           }
           fut = cur + (spe * dir);
           pnow[i] = fut;
           faboPWM.set_channel_value(i, fut);
           Serial.print(i);
           Serial.print(": "); 
           Serial.print(fut);
           Serial.print(" -> ");
           Serial.println(des);
        }
   }
}

void servo_test(void)
{
    int mmin, mmax, ch, now;
    String temp, prev;
    temp=rxBuf;
    
    temp.remove(1);
    ch=temp.toInt();

    temp=rxBuf;
    temp.remove(0,1);
    temp.remove(3);
    mmin=temp.toInt();

    temp=rxBuf;
    temp.remove(0,4);
    temp.remove(3);
    mmax=temp.toInt();  

Serial.print(ch);
Serial.print(": ");
Serial.print(mmin);
Serial.print(" <-> ");
Serial.println(mmax);

  if((mmin<mmax)&&(mmin>0)&&(mmax>0))
  {
    for(int i=0; i<2; i++)
    {
       now = mmin;
       while(now<mmax)
       {
          now=now+1;
          delay(20);
          faboPWM.set_channel_value(ch, now);
          Serial.println(now);
       }
       while(now>mmin)
       {
          now=now-1;
          delay(20);
          faboPWM.set_channel_value(ch, now);
          Serial.println(now);
       } 
    }
    CAN0.readMsgBuf(&rxId, &len, rxBuf);
    Serial.println("done");
  }      
}

void servo_drive(void)
{
    String msg1, msg2, msg3, temp, work;
    int ut, smin, smax, ssafe, tgt, id1, id2, id3, sid1, sid2, curr, err, maxerr=10, erf=0;
    
    msg1=rxBuf;
    id1=rxId;
    Serial.print("  msg1 = ");
    Serial.println(msg1);
    err=0;
    do{
        CAN0.readMsgBuf(&rxId, &len, rxBuf); 
        id2=rxId;
        err=err+1;  
        //Serial.print(" ");
        //Serial.print(rxId);
    }while((id2!=17)||(err==maxerr));
    msg2=rxBuf;
    if(err==maxerr)
    {
       erf=1;
    }
    err=0;
    Serial.println();
    Serial.print("  msg2 = ");
    Serial.println(msg2);
    do{
        CAN0.readMsgBuf(&rxId, &len, rxBuf);
        id3=rxId; 
        //Serial.print(" ");
        //Serial.print(rxId);  
    }while((id3!=18)||(err==maxerr));
    msg3=rxBuf;
    if(err==maxerr)
    {
       erf=1;
    }

    if(erf==0)
    {
        err=0;
        Serial.println();
        Serial.print("  msg3 = ");
        Serial.println(msg3);
        
        temp=msg1;
        temp.remove(1);  
        sid1=temp.toInt();
    
        temp=msg2;
        temp.remove(1);  
        sid2=temp.toInt();
    
        if(sid1==sid2)
        {
          temp=msg1;
          temp.remove(0,1);
          temp.remove(3);
          smin=temp.toInt();
          temp=msg1;
          temp.remove(0,4);
          temp.remove(3);
          smax=temp.toInt();
          temp=msg2;
          temp.remove(0,1);
          temp.remove(3);
          ssafe=temp.toInt();
          temp=msg2;
          temp.remove(0,4);
          temp.remove(3);
          tgt=temp.toInt();
          msg3.remove(8);
          ut=msg3.toInt();
          // ut smin smax ssafe tgt
    
          Serial.print(" -> S = ");
          Serial.print(sid1);
    
          Serial.print("  L = ");
          Serial.print(smin);
    
          Serial.print("  K = ");
          Serial.print(ssafe);
    
          Serial.print("  M = ");
          Serial.print(smax);
    
          Serial.print("  V = ");
          Serial.print(ut);
    
          Serial.print("  TGT = ");
          Serial.println(tgt);
          Serial.println();
          Serial.println();
          
          if(sid1<servo_nr)
          {
            curr = pnow[sid1];           // check the current position of servo
            if((curr!=tgt))                // do something if it is not in the desired position;
            {
                if((curr<smax)&&(curr>smin)&&(tgt<=smax)&&(tgt>=smin))    //if start and stop positions are within limits
                {
                   pdes[sid1] = tgt;                                         //set servo target to the desired value
                   pspe[sid1] = ut;
                }
                else                      // if movement parameters are out of bond go to a safe position (rest)
                {
                   pnow[sid1] = ssafe;
                   pdes[sid1] = ssafe; 
                }
              }
           }
        }
   }
   else
   {
          Serial.println();
          Serial.print("EXIT WITH ERROR!");  
          Serial.println();  
   }
}

void loop() 
{
   if(!digitalRead(CAN0_INT))                         // If CAN0_INT pin is low, read receive buffer
   {
      CAN0.readMsgBuf(&rxId, &len, rxBuf);
      if(rxId==38)
      {
         servo_test();
      }
      if(rxId==16)
      {
         servo_drive();
      }
   
   }
   move_servo();
   delay(20);
}
Torna in alto