Robot Idea

Mickey666Maus

Germany
Skip to comments

Hey guys...this Adafruit 16-channel PWM/Servo Shield integrates seamlessly to ARC now...
So if you are having one sitting on the shelf somewhere, give it a try and use some ARC functionality to drive your servos!!:)

User-inserted image

By — Last update
#1  

Code:

#include <Servo.h>
#include "SendOnlySoftwareSerial.h"
// --------------------------------------------------------------------------
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)
uint8_t servonum = 0;
// --------------------------------------------------------------------------
// The first digital port that is usable on this controller
//#define _PortStart 2
#define _PortStart 0
// The last digital port on this controller that is usable
#define _PortCnt 15
// The number of analog ports
#define _AnalogPorts 6
// The firmware version that is reported to ARC to notify of capabilities
#define _FIRMWARE_ID 0x00000005
// The communication baud rate
#define _BAUD_RATE 57600
// The primary communication interface between ARC and this controller
#define COMMUNICATION_PORT Serial
// The amount of RX buffer on the communication interface for ARC
#define _BUFFER_SIZE 1024
// --------------------------------------------------------------------------
byte         _INPUT_BUFFER[_BUFFER_SIZE];
unsigned int _WRITE_POSITION = 0;
unsigned int _READ_POSITION = 0;
int _BAUD_RATES [] = {
  4800,
  9600,
  19200,
  38400,
  57600,
  115200,
  115200
};
Servo Servos[_PortCnt];
#define CmdUnknown            0
#define CmdReleaseAllServos   1
#define CmdGetUniqueID        2
#define CmdEZBv3              3
#define CmdEZBv4              4
#define CmdSoundBeep          5
#define CmdEZServo            6
#define CmdI2CWrite           10
#define CmdI2CRead            11
#define CmdBootLoader         14
#define CmdSetPWMSpeed        15
#define CmdSetServoSpeed      39
#define CmdPing               0x55
#define CmdSetDigitalPortOn   100
#define CmdSetDigitalPortOff  124
#define CmdGetDigitalPort     148
#define CmdSetServoPosition   172
#define CmdGetADCValue        196
#define CmdSendSerial         204
#define CmdHC_SR04            228
#define CmdGetFirwareID       253
#define CmdSoundStreamCmd     254
// CmdEZBv4 Commands
// ----------------------------------------------------------------------------------
#define CmdV4SetLipoBatteryProtectionState 0
#define CmdV4SetBatteryMonitorVoltage      1
#define CmdV4GetBatteryVoltage             2
#define CmdV4GetCPUTemp                    3
#define CmdV4UARTExpansion0Init            5
#define CmdV4UARTExpansion0Write           6
#define CmdV4UARTExpansion0AvailableBytes  7
#define CmdV4UARTExpansion0Read            8
#define CmdV4UARTExpansion1Init            9
#define CmdV4UARTExpansion1Write           10
#define CmdV4UARTExpansion1AvailableBytes  11
#define CmdV4UARTExpansion1Read            12
#define CmdV4UARTExpansion2Init            13
#define CmdV4UARTExpansion2Write           14
#define CmdV4UARTExpansion2AvailableBytes  15
#define CmdV4UARTExpansion2Read            16
#define CmdV4I2CClockSpeed                 17
#define CmdV4UARTClockSpeed                18
#define CmdV4ResetToDefaults               19
// CmdSoundStreamCmd Commands
// ----------------------------------------------------------------------------------
#define CmdSoundInitStop 0
#define CmdSoundLoad     1
#define CmdSoundPlay     2
bool IsAvail() {
  return _WRITE_POSITION != _READ_POSITION || COMMUNICATION_PORT.available();
}
byte ReadByte() {
  while (_WRITE_POSITION == _READ_POSITION && COMMUNICATION_PORT.available() == 0);
  while (COMMUNICATION_PORT.available()) {
    _WRITE_POSITION++;
    _INPUT_BUFFER[_WRITE_POSITION % _BUFFER_SIZE] = COMMUNICATION_PORT.read();
  }
  _READ_POSITION++;
  return _INPUT_BUFFER[_READ_POSITION % _BUFFER_SIZE];
}
void setup() {
  COMMUNICATION_PORT.begin(_BAUD_RATE);
  // ----------------------------------------------------------------------------------
  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  delay(10);
 // ----------------------------------------------------------------------------------
 
}
// ----------------------------------------------------------------------------------
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;  // convert to us
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}
// ----------------------------------------------------------------------------------
void loop() {
  doEZProtocol();
}
void Write32(long val) {
  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 16) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 24) & 0xff));
}
void Write16(int val) {
  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
}
#define UNKNOWN_PIN 0xFF
uint8_t getPinMode(uint8_t pin) {
  uint8_t bit = digitalPinToBitMask(pin);
  uint8_t port = digitalPinToPort(pin);
  // I don't see an option for mega to return this, but whatever...
  if (NOT_A_PIN == port)
    return UNKNOWN_PIN;
  // Is there a bit we can check?
  if (0 == bit)
    return UNKNOWN_PIN;
  // Is there only a single bit set?
  if (bit & bit - 1)
    return UNKNOWN_PIN;
  volatile uint8_t *reg, *out;
  reg = portModeRegister(port);
  out = portOutputRegister(port);
  if (*reg & bit)
    return OUTPUT;
  else if (*out & bit)
    return INPUT_PULLUP;
  else
    return INPUT;
}
void doEZProtocol() {
  if (IsAvail()) {
    byte cmd = ReadByte();
    if (cmd == CmdPing) {
      // return as a "Capability Controller"
      COMMUNICATION_PORT.write(222);
    } else if (cmd == CmdGetFirwareID) {
      Write32(_FIRMWARE_ID);
    } else if (cmd == CmdReleaseAllServos) {
      for (int port = _PortStart; port < _PortCnt; port++)
        
        pwm.setPWM(port, 0, 0);
        
        //if (Servos[port].attached())
        //  Servos[port].detach();
        
    } else if (cmd >= CmdSetServoPosition && cmd <= CmdSetServoPosition + 23) {
      byte port = cmd - CmdSetServoPosition;
      byte pos = ReadByte();
      if (port >= _PortStart && port <= _PortCnt) {
        
        //if (pos == 0 && Servos[port].attached()) {
        
        //  Servos[port].detach();
        //  if (pos == 0)
        
        if (pos == 0) {
          
          pwm.setPWM(port, 0, 0);
          
        } else {
          pwm.setPWM(port, 0, ((int)(pos * 2.5) + 130));
          
          //if (!Servos[port].attached())
          //  Servos[port].attach(port);
          //Servos[port].write(pos);
        }
      }
    } else if (cmd >= CmdSetPWMSpeed && cmd <= CmdSetPWMSpeed + 23) {
      byte port = cmd - CmdSetPWMSpeed;
      byte pos = ReadByte();
      if (port >= _PortStart && port <= _PortCnt) {
        if (Servos[port].attached())
          Servos[port].detach();
        if (getPinMode(port) != OUTPUT)
          pinMode(port, OUTPUT);
        analogWrite(port, map(pos, 0, 100, 0, 255));
      }
    } else if (cmd >= CmdSetDigitalPortOn && cmd <= CmdSetDigitalPortOn + 23) {
      byte port = cmd - CmdSetDigitalPortOn;
      if (port >= _PortStart && port <= _PortCnt) {
        if (Servos[port].attached())
          Servos[port].detach();
        if (getPinMode(port) != OUTPUT)
          pinMode(port, OUTPUT);
        digitalWrite(port, HIGH);
      }
    } else if (cmd >= CmdSetDigitalPortOff && cmd <= CmdSetDigitalPortOff + 23) {
      byte port = cmd - CmdSetDigitalPortOff;
      if (port >= _PortStart && port <= _PortCnt) {
        if (Servos[port].attached())
          Servos[port].detach();
        if (getPinMode(port) != OUTPUT)
          pinMode(port, OUTPUT);
        digitalWrite(port, LOW);
      }
    } else if (cmd >= CmdGetDigitalPort && cmd <= CmdGetDigitalPort + 23) {
      byte port = cmd - CmdGetDigitalPort;
      if (port >= _PortStart && port <= _PortCnt) {
        if (Servos[port].attached())
          Servos[port].detach();
        if (getPinMode(port) != INPUT)
          pinMode(port, INPUT);
        COMMUNICATION_PORT.write(digitalRead(port));
      } else {
        COMMUNICATION_PORT.write(0);
      }
    } else if (cmd >= CmdGetADCValue && cmd <= CmdGetADCValue + 7) {
      byte port = cmd - CmdGetADCValue;
      if (port >= 0 && port <= _AnalogPorts)
        Write16(analogRead(port));
      else
        Write16(0);
    } else if (cmd >= CmdSendSerial && cmd <= CmdSendSerial + 23) {
      // Send Serial
      uint8_t port = cmd - CmdSendSerial;
      uint8_t baud = ReadByte(); // Baud rate
      uint8_t size = ReadByte(); // Size
      if (port >= _PortStart && port <= _PortCnt) {
        if (Servos[port].attached())
          Servos[port].detach();
        SendOnlySoftwareSerial tmpSerial(port);
        tmpSerial.begin(_BAUD_RATES[baud]);
        for (int x = 0; x < size; x++)
          tmpSerial.write(ReadByte());
        tmpSerial.end();
      }
    }
  }
}
#3   — Edited
Sweet!! I am really happy with the progress!!:)

One thing where I am a little stuck is, since I connected my 16-Channel servo Driver (which basically has the same pinout and functionality than the shield) via I2C to my Raspberry Pi...how can I control the board by connecting to my EZBPi?
It would be great if this could be possible!!:) 

This is the board connected to my Raspberry Pi via I2C
https://learn.adafruit.com/16-channel-pwm-servo-driver?view=all
Synthiam
#4   — Edited
I wonder if the Alamode would work if you needed a go between? I know that it interfaces between the world of Arduino and raspberry pi but I haven't looked at it in depth to see how it works. I believe it uses an I2C bootloader interfaced to the Pi. My thought was that you might be able to use the Arduino shield you posted at the beginning of this thread on top of the Alamode. Alternatively, you might be able to hook up an Arduino + above shield to the Pi USB and use serial communication functionality as well. Just ideas, not really a well thought out solution.

Link to the Alamode at Seeed Studios
User-inserted image
#5  
@Jeremie Thanks for the feedback!!:) But unfortunately, I don't think this is an option! I am using the board for the reason that I already connected a shield to my Raspberry Pi...so I was hoping for a solution which allows me to connect straight out of ARC to the EZBPi and have it taking care of the connection the same way my Raspberry Pi is doing it. (I2C)
But if this is not possible for now, I can still use the EZBPi functionality to drive Serial Bus Servos which is also fun!!:)
Synthiam
#6  
The arduino connects to the raspberry pi via usb and becomes a serial port

you connect to that serial port in ARC.
#7  
@DJSures I was trying to do it this way but had no luck connecting while running ARC on my Raspberry Pi! But it could have been that I chose the wrong COM port or another PEBCAK...:D I will try again later when I a back home!!

My Raspberry Pi is connecting to the Server Board using its I2C ports...
If I have to use a Raspberry Pi as a bridge, I would need to change the wiring everytime I am changing setups...
eg my Raspi only solution and the EZBPi solution.

Its not a big deal, I will just solder the I2C with open endings to the servo Board, and plug them either to the Arduino or to the Raspi...
I was just hoping it could be done by the EZBPi talk to the servo Board directly!!

I am aware of the situation that I am probably the only guy here using this setup, so don't worry, I can plug and unplug a cable every now and then. It's great that I can run my robot on ARC now without too much re-wiring!!:)
Synthiam
#8   — Edited
I'm 100% confident that the reason you can’t get it to work is not connecting to the right serial port - and most likely the incorrect baud rate as well. 

Both of those settings need to be selected that match the Arduino.
#9  
Yup...I was kinda in a hurry!! On my PC all works just fine!!:D
#10   — Edited
Sorry to ask again, but I run into this problem when trying to connect the ARaspberry Pi to an Arduino Uno with ARC...
I am trying to connect on this port /dev/ttyACM0 and with this Baudrate 57600

3/5/2019 11:46:00 PM - Attempting connection on /dev/ttyACM0
3/5/2019 11:46:00 PM - Connection Failed: System.NotImplementedException: The method or operation is not implemented.
  at System.IO.Ports.SerialPort.set_DiscardNull (System.Boolean value) [0x00000] in <bd46d4d4f7964dfa9beea098499ab597>:0 
  at (wrapper remoting-invoke-with-check) System.IO.Ports.SerialPort:set_DiscardNull (bool)
  at EZ_B.EZB.Connect (System.String hostname, System.Int32 baudRate) [0x001e1] in <d1fda7531a8e420f99237291dee29592>:0 
3/5/2019 11:46:00 PM - Disconnected
Netherlands
#11   — Edited
Still a newbie... 

Is it possible to connect it to the ez-b controller? 
Or do i need to use a arduino?
Because i can't see it in the "add control" list...
#12  
You need an Arduino, you basically turn an Arduino into an EZ-B!!
Synthiam
#13  
Mickey’s correct - it’s an arduino shield. So the fridge is only connects to a arduino. The form factor requires an arduino