Robot Idea

Mickey666Maus

Germany
Skip to comments

Hey guys...this Adafruit 16-channel PWM/Servo Shield integrates seamlessly to EZ-Builder now...
So if you are having one sitting on the shelf somewhere, give it a try and use some EZ-Builder 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 EZ-Builder to notify of capabilities
#define _FIRMWARE_ID 0x00000005
// The communication baud rate
#define _BAUD_RATE 57600
// The primary communication interface between EZ-Builder and this controller
#define COMMUNICATION_PORT Serial
// The amount of RX buffer on the communication interface for EZ-Builder
#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();
      }
    }
  }
}
#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 EZ-Builder 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 EZ-Builder.
#7  
@DJSures I was trying to do it this way but had no luck connecting while running EZ-Builder 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 EZ-Builder 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 EZ-Builder...
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