BasicMicroUK - Forums

www.basicmicro.co.uk
It is currently Sun Nov 19, 2017 11:40 am

All times are UTC [ DST ]




Post new topic Reply to topic  [ 5 posts ] 
Author Message
 Post subject: Implementing PID Position control
PostPosted: Sat Sep 27, 2014 11:56 pm 
Offline
New User

Joined: Sat Sep 27, 2014 10:41 pm
Posts: 3
Hi,

I urgently need help with my setup please!

I have a linear actuator with built in potentiometer feedback. I have connected this to a 2V supply and the wiper to pin EN1.

I want to control the length of the actuator using the PID controller on my RoboClaw 30A using the USB interface.

Here is my approach in software:
Start-up and connect over USB
Set encoder mode to 0x81
Set Velocity PID parameters (even though i am not controlling the velocity, but position - do i need this?!)
Set Position PID parameters
Reset encoders
Set the target position
After setting position sit in while loop and read feedback.


Code:
#include "roboclaw_driver/RoboClaw.h"
#include "roboclaw_driver/motor_state.h"
#include "roboclaw_driver/claw_state.h"
#include <ros/ros.h>
#include <ros/console.h>
#include "std_msgs/String.h"

#define ADDRESS 0x80

using namespace std;

class Driver {

private:
  ros::NodeHandle nh_;
  ros::Publisher pub_;

  string port;
  USBSerial serial;
  RoboClaw claw_;

  struct claw_properties_t {
    string version;
    int year;
  } claw_properties;

  int address_;
  int serial_errs_;

public:

    explicit Driver(const ros::NodeHandle &node)
  {

      pub_ = nh_.advertise<roboclaw_driver::claw_state>("claw_state", 5);
    bool valid;
    }

    ~Driver() {
    }

void getState(void)
{
    roboclaw_driver::claw_state state_;
    uint8_t status;
    bool valid;

    int16_t m1_current, m2_current;
    uint8_t mode1=5, mode2=5;

    valid = claw_.ReadEncMode(ADDRESS, mode1, mode2);

    state_.m1_enc = claw_.ReadEncM1(ADDRESS, &status,&valid);
    state_.m2_enc = claw_.ReadEncM2(ADDRESS, &status,&valid);
    state_.v = claw_.ReadSpeedM1(ADDRESS, &status, &valid);
    state_.w = state_.m1_enc;
    uint32_t p,i,d,qpps;
    claw_.ReadPIDM1(ADDRESS, p, i, d, qpps );
    state_.left_pid_pe = p;
    state_.left_pid_ie = i;
    state_.left_pid_de = d;
    state_.left_qpps = qpps;
    claw_.ReadPIDM2(ADDRESS, p, i, d, qpps );
    state_.right_pid_pe = p;
    state_.right_pid_ie = i;
    state_.right_pid_de = d;
    state_.right_qpps = qpps;
    state_.main_voltage = claw_.ReadMainBatteryVoltage(ADDRESS,&valid);
    state_.logic_voltage = claw_.ReadLogicBattVoltage(ADDRESS,&valid);
    claw_.ReadCurrents(ADDRESS, m1_current, m2_current);
    state_.m1_current = m1_current;
    state_.m2_current = m2_current;

    pub_.publish(state_);
}

void basic() {
  ros::NodeHandle nh;
  uint8_t status;
  bool valid;

  port = "/dev/roboclaw";//TODO: Don't hardcode port

  serial.Open(port.c_str());
  claw_ = RoboClaw(&serial);

  //Use Absolute Encoder and Enable encoders in RC mode
  claw_.SetM1EncoderMode(ADDRESS,3);
  claw_.SetM2EncoderMode(ADDRESS,3);

  //Set PID Coefficients
  claw_.SetM1Constants(ADDRESS,0X00004000,0x00010000 ,0x00008000,3600);
  claw_.SetM2Constants(ADDRESS,0X00004000,0x00010000 ,0x00008000,3600);

  claw_.SetM1PstnConsts(ADDRESS,20000,2000,4,10000,10,0,21000);//D P I IMAX DEAD MIN MAX
  claw_.SetM2PstnConsts(ADDRESS,20000,2000,4,10000,10,0,21000);//D P I IMAX DEAD MIN MAX

  //Save settings in Roboclaw eeprom.  Uncomment to save settings.
  //roboclaw.WriteNVM(address);

  claw_.ResetEncoders(ADDRESS);

  uint32_t accel = 0, decel = accel, vel=1200, pstn=1300;
    claw_.SpeedAccelDeccelPositionM1(ADDRESS,accel,vel,decel,pstn,1);
    claw_.SpeedAccelDeccelPositionM2(ADDRESS,accel,vel,decel,pstn,1);

    claw_.SpeedAccelDistanceM1(ADDRESS, 0, 3600, 100, 1);
    claw_.SpeedAccelDistanceM2(ADDRESS, 0, 3600, 100, 1);

    while (ros::ok()) {
      ros::Duration(0.5).sleep();

      getState();

      ros::spinOnce();
    }
}
};//End class

int main(int argc, char **argv) {
  ros::init(argc, argv, "enc_demo");
  ros::NodeHandle n;

  Driver driver(n);
  driver.basic();
}


Top
 Profile  
 
 Post subject: Re: Implementing PID Position control
PostPosted: Sun Sep 28, 2014 9:41 am 
Offline
New User

Joined: Sat Sep 27, 2014 10:41 pm
Posts: 3
Update:
I have found that if i set the parameters using the Ion Motion package in Windows i can store the settings and then send the controller the position set point in linux, but why can't i set these parameters from the c++ code above...?


Top
 Profile  
 
 Post subject: Re: Implementing PID Position control
PostPosted: Tue Oct 21, 2014 4:35 pm 
Offline
Site Admin
User avatar

Joined: Thu Mar 01, 2001 7:00 pm
Posts: 1316
Location: Temecula, CA
Im assuming this is a linux box? Where did you get the roboclaw interface library? Link please.

Can you read the version information from the Roboclaw using your code? The most recent firmware changed the getstatus command which now returns 2 bytes instead of one. This could break third party libraries(until they are fixed to read the second byte as well).

_________________
Tech Support
Basic Micro - Robotic Technology Evolved


Top
 Profile  
 
 Post subject: Re: Implementing PID Position control
PostPosted: Tue Oct 21, 2014 11:55 pm 
Offline
New User

Joined: Sat Sep 27, 2014 10:41 pm
Posts: 3
Hi,

I use the Arduino CPP library as suggested by Pololu which links to the Orion Robotics server: http://downloads.orionrobotics.com/downloads/code/arduino.zip

I don't have issues reading the version, the code i have to do so is:
Code:
bool RoboClaw::ReadVersion(uint8_t address, string *version) {
  version->resize(32);

  uint8_t crc;
  write(address);
  crc=address;
  write(GETVERSION);
  crc+=GETVERSION;

  for(uint8_t i=0;i<32;i++) {
    (*version)[i] = read();
    crc += (*version)[i];
    if ((*version)[i] == 0) {
      return (crc&0x7F) == read();
    }
  }
  return false;
}


This function indicates that I have version 4.1.2.
Is this the latest version?

@Acidtech, What functions in specif have changed?

Is there an updated library available?

Regards,

Chris.


Top
 Profile  
 
 Post subject: Re: Implementing PID Position control
PostPosted: Mon Nov 10, 2014 8:19 pm 
Offline
Site Admin
User avatar

Joined: Thu Mar 01, 2001 7:00 pm
Posts: 1316
Location: Temecula, CA
Ok. That is the latest released version. In your first post you have WriteNVM commented out. Uncomment it. Run your program till it gets past the WriteNVM command and then power down. Disconnect the roboclaw from the arduino, power cycle the roboclaw and connect it to the PC and check its settings using IonMotion. Tell me what has or has not been saved.

Also just to be safe I recommend you download the arduino libraries directly from our website. Pololu's copies of our libraries could be out of date.

_________________
Tech Support
Basic Micro - Robotic Technology Evolved


Top
 Profile  
 
Display posts from previous:  Sort by  
Post new topic Reply to topic  [ 5 posts ] 

All times are UTC [ DST ]


Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Jump to:  
cron
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group

phpBB SEO