mirror of
https://github.com/eclipse/upm.git
synced 2025-03-15 04:57:30 +03:00
smartdrive: upm implementation for SmartDrive from openelectronics.com
Signed-off-by: Oussema Harbi <oussema.elharbi@gmail.com> Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
This commit is contained in:
Oussema Harbi
Mihai Tudor Panu
parent
4381f8bd2e
commit
7e402ede34
73
examples/c++/smartdrive.cxx
Normal file
73
examples/c++/smartdrive.cxx
Normal file
@ -0,0 +1,73 @@
|
||||
/*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Author: Oussema Harbi <oussema.elharbi@gmail.com>
|
||||
* Copyright (c) <2016> <Oussema Harbi>
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||||
* this software and associated documentation files (the "Software"), to deal in
|
||||
* the Software without restriction, including without limitation the rights to
|
||||
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
|
||||
* the Software, and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
|
||||
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
|
||||
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
|
||||
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
|
||||
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
#include <iostream>
|
||||
#include "smartdrive.h"
|
||||
#include <signal.h>
|
||||
|
||||
upm::SmartDrive *drive = NULL;
|
||||
|
||||
void
|
||||
sig_handler(int signo)
|
||||
{
|
||||
printf("got signal\n");
|
||||
if (signo == SIGINT) {
|
||||
printf("exiting application\n");
|
||||
if (drive != NULL)
|
||||
delete drive;
|
||||
exit (0);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
main(int argc, char **argv)
|
||||
{
|
||||
float voltage = 0;
|
||||
|
||||
cout << "SmartDrive demo is starting. Please make sure drive is connected to board" << endl;
|
||||
sleep(2); //Wait for 2 seconds in case you want to fix your h/w setup
|
||||
|
||||
// Instantiate a SmartDrive connected to /dev/i2c-0 bus, using DefaultAddress
|
||||
drive = new upm::SmartDrive(0);
|
||||
|
||||
cout << "Battery Voltage before motor run : " << drive.GetBattVoltage() << std::endl;
|
||||
//Set motor M1 to run for 300seconds, with speed of 15RPM, waith for it to finish and then Brake It
|
||||
drive->Run_Seconds(Motor_ID_M1, Dir_Forward, 15, 300, true, Action_Brake );
|
||||
std::cout << "Battery Voltage after motor run : " << drive.GetBattVoltage() << std::endl;
|
||||
//Rotate motor M2 2270 degrees, in reverse sens, with speed of 10RPM, return immediately from function call
|
||||
drive->Run_Degrees(Motor_ID_M2, Dir_Reverse, 10, 2270, false, Action_Float);
|
||||
//While motor is running, Display its status
|
||||
drive->PrintMotorStatus(Motor_ID_M2);
|
||||
sleep(2); //Sleep for 2 seconds
|
||||
//Stop motor M2 and then finish program
|
||||
drive->StopMotor(Motor_ID_M2, Action_BrakeHold );
|
||||
|
||||
std::cout << "Demo complete. GoodBye" << std::endl;
|
||||
|
||||
delete drive;
|
||||
drive = NULL;
|
||||
|
||||
return 0;
|
||||
}
|
||||
5
src/smartdrive/CMakeLists.txt
Normal file
5
src/smartdrive/CMakeLists.txt
Normal file
@ -0,0 +1,5 @@
|
||||
set (libname "smartdrive")
|
||||
set (libdescription "upm SmartDrive")
|
||||
set (module_src ${libname}.cxx)
|
||||
set (module_h ${libname}.h)
|
||||
upm_module_init()
|
||||
8
src/smartdrive/javaupm_smartdrive.i
Normal file
8
src/smartdrive/javaupm_smartdrive.i
Normal file
@ -0,0 +1,8 @@
|
||||
%module javaupm_smartdrive
|
||||
%include "../upm.i"
|
||||
|
||||
%{
|
||||
#include "smartdrive.h"
|
||||
%}
|
||||
|
||||
%include "smartdrive.h"
|
||||
8
src/smartdrive/jsupm_smartdrive.i
Normal file
8
src/smartdrive/jsupm_smartdrive.i
Normal file
@ -0,0 +1,8 @@
|
||||
%module jsupm_smartdrive
|
||||
%include "../upm.i"
|
||||
|
||||
%{
|
||||
#include "smartdrive.h"
|
||||
%}
|
||||
|
||||
%include "smartdrive.h"
|
||||
9
src/smartdrive/pyupm_smartdrive.i
Normal file
9
src/smartdrive/pyupm_smartdrive.i
Normal file
@ -0,0 +1,9 @@
|
||||
%module pyupm_smartdrive
|
||||
%include "../upm.i"
|
||||
|
||||
%include "stdint.i"
|
||||
|
||||
%include "smartdrive.h"
|
||||
%{
|
||||
#include "smartdrive.h"
|
||||
%}
|
||||
430
src/smartdrive/smartdrive.cxx
Normal file
430
src/smartdrive/smartdrive.cxx
Normal file
@ -0,0 +1,430 @@
|
||||
/*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Author: Oussema Harbi <oussema.elharbi@gmail.com>
|
||||
* Copyright (c) <2016> <Oussema Harbi>
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||||
* this software and associated documentation files (the "Software"), to deal in
|
||||
* the Software without restriction, including without limitation the rights to
|
||||
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
|
||||
* the Software, and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
|
||||
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
|
||||
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
|
||||
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
|
||||
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <exception>
|
||||
#include <stdexcept>
|
||||
#include <unistd.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "smartdrive.h"
|
||||
|
||||
|
||||
using namespace upm;
|
||||
|
||||
SmartDrive::SmartDrive(int i2c_bus, int address): m_smartdrive_control_address(address), m_i2c_smartdrive_control(i2c_bus)
|
||||
{
|
||||
mraa_result_t ret = m_i2c_smartdrive_control.address(m_smartdrive_control_address);
|
||||
if (ret != MRAA_SUCCESS) {
|
||||
throw std::invalid_argument(std::string(__FUNCTION__) +
|
||||
": mraa_i2c_address() failed");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::writeByte(uint8_t addr, uint8_t value) {
|
||||
try {
|
||||
m_i2c_smartdrive_control.writeReg(addr, value);
|
||||
} catch (int e) {
|
||||
std::cout << "Failed to write " << value << " to address " << addr << " --> " << e << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t
|
||||
SmartDrive::readByte(uint8_t addr) {
|
||||
try {
|
||||
return m_i2c_smartdrive_control.readReg(addr);
|
||||
} catch (int e) {
|
||||
std::cout << "Failed to read byte at address " << addr << " --> " << e << std::endl;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void
|
||||
SmartDrive::writeArray(uint8_t* array) {
|
||||
try {
|
||||
m_i2c_smartdrive_control.write(array, sizeof(array)/sizeof(uint8_t));
|
||||
} catch (int e) {
|
||||
std::cout << "Failed to write array values to address " << array[0] << " --> " << e << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t
|
||||
SmartDrive::readInteger(uint8_t addr) {
|
||||
try {
|
||||
return m_i2c_smartdrive_control.readWordReg(addr);
|
||||
} catch (int e) {
|
||||
std::cout << "Failed to read value at address " << addr << " --> " << e << std::endl;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint32_t
|
||||
SmartDrive::readLongSigned(uint8_t addr) {
|
||||
uint8_t bytes[4]={0};
|
||||
|
||||
try {
|
||||
m_i2c_smartdrive_control.readBytesReg(addr, bytes, sizeof(bytes)/sizeof(uint8_t));
|
||||
return (bytes[0]|(bytes[1]<<8)|(bytes[2]<<16)|(bytes[3]<<24));
|
||||
} catch (int e) {
|
||||
std::cout << "Failed to read integer value at address " << addr << " --> " << e << std::endl;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void
|
||||
SmartDrive::command(uint8_t cmd) {
|
||||
std::cout << "Running Command : " << cmd << std::endl;
|
||||
writeByte(SmartDrive_COMMAND, cmd);
|
||||
}
|
||||
|
||||
|
||||
float
|
||||
SmartDrive::GetBattVoltage() {
|
||||
uint8_t value = 0;
|
||||
try {
|
||||
value = readByte(SmartDrive_BATT_VOLTAGE);
|
||||
return (value * SmartDrive_VOLTAGE_MULTIPLIER);
|
||||
} catch (int e) {
|
||||
std::cout << "Error: Could not read voltage -> " << e << std::endl;
|
||||
}
|
||||
return -1.0f;
|
||||
}
|
||||
|
||||
|
||||
uint32_t
|
||||
SmartDrive::ReadTachometerPosition(int motor_id) {
|
||||
try {
|
||||
if (motor_id == 1 )
|
||||
return readLongSigned(SmartDrive_POSITION_M1);
|
||||
else
|
||||
return readLongSigned(SmartDrive_POSITION_M2);
|
||||
} catch (int e) {
|
||||
std::cout << "Error: Could not read encoders" << std::endl;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::Run_Unlimited(int motor_id, int direction, uint8_t speed) {
|
||||
uint8_t ctrl = 0;
|
||||
ctrl |= SmartDrive_CONTROL_SPEED;
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
|
||||
std::cout << "Running with speed : " << (int) speed << std::endl;
|
||||
|
||||
if ( motor_id != SmartDrive_Motor_ID_BOTH )
|
||||
ctrl |= SmartDrive_CONTROL_GO;
|
||||
if ( direction != SmartDrive_Dir_Forward )
|
||||
speed = speed * -1;
|
||||
if ( motor_id != SmartDrive_Motor_ID_2) {
|
||||
uint8_t array [5] = {SmartDrive_SPEED_M1, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_1) {
|
||||
uint8_t array [5] = {SmartDrive_SPEED_M2, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id == SmartDrive_Motor_ID_BOTH )
|
||||
writeByte(SmartDrive_COMMAND, CMD_S);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::StopMotor(int motor_id, int next_action ) {
|
||||
if ( next_action !=SmartDrive_Action_Float )
|
||||
writeByte(SmartDrive_COMMAND, CMD_A+motor_id-1);
|
||||
else
|
||||
writeByte(SmartDrive_COMMAND, CMD_a+motor_id-1);
|
||||
}
|
||||
|
||||
void
|
||||
SmartDrive::Run_Seconds(int motor_id, int direction, uint8_t speed, uint8_t duration, bool wait_for_completion, int next_action ) {
|
||||
uint8_t ctrl = 0;
|
||||
ctrl |= SmartDrive_CONTROL_SPEED;
|
||||
ctrl |= SmartDrive_CONTROL_TIME;
|
||||
|
||||
if ( next_action ==SmartDrive_Action_Brake )
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
if ( next_action ==SmartDrive_Action_BrakeHold ) {
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
ctrl |= SmartDrive_CONTROL_ON;
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_BOTH )
|
||||
ctrl |= SmartDrive_CONTROL_GO;
|
||||
if ( direction != SmartDrive_Dir_Forward )
|
||||
speed = speed * -1;
|
||||
if ( motor_id != SmartDrive_Motor_ID_2) {
|
||||
uint8_t array[5] = {SmartDrive_SPEED_M1, speed, duration, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_1) {
|
||||
uint8_t array[5] = {SmartDrive_SPEED_M2, speed, duration, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id == SmartDrive_Motor_ID_BOTH )
|
||||
writeByte(SmartDrive_COMMAND, CMD_S);
|
||||
if ( wait_for_completion ) {
|
||||
sleep(1); //this delay is required for the status byte to be available for reading.
|
||||
WaitUntilTimeDone(motor_id);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::WaitUntilTimeDone(int motor_id) {
|
||||
while (IsTimeDone(motor_id) == false)
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
SmartDrive::IsTimeDone(int motor_id) {
|
||||
uint8_t result_1 = 0, result_2 = 0;
|
||||
if ( motor_id != SmartDrive_Motor_ID_2 )
|
||||
result_1 = readByte(SmartDrive_STATUS_M1);
|
||||
if ( motor_id != SmartDrive_Motor_ID_1 )
|
||||
result_2 = readByte(SmartDrive_STATUS_M2);
|
||||
return (((result_1 & 0x40) == 0) && ((result_2 & 0x40) == 0) ); //look for time bits to be zero
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::Run_Degrees(int motor_id, int direction, uint8_t speed, uint32_t degrees, bool wait_for_completion, int next_action) {
|
||||
uint8_t ctrl = 0;
|
||||
ctrl |= SmartDrive_CONTROL_SPEED;
|
||||
ctrl |= SmartDrive_CONTROL_TACHO;
|
||||
ctrl |= SmartDrive_CONTROL_RELATIVE;
|
||||
|
||||
uint32_t d = degrees;
|
||||
if ( direction != SmartDrive_Dir_Forward )
|
||||
d = degrees * -1 ;
|
||||
|
||||
uint8_t t4 = (d/0x1000000);
|
||||
uint8_t t3 = ((d%0x1000000)/0x10000);
|
||||
uint8_t t2 = (((d%0x1000000)%0x10000)/0x100);
|
||||
uint8_t t1 = (((d%0x1000000)%0x10000)%0x100);
|
||||
|
||||
if ( next_action ==SmartDrive_Action_Brake )
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
if ( next_action ==SmartDrive_Action_BrakeHold ) {
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
ctrl |= SmartDrive_CONTROL_ON;
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_BOTH )
|
||||
ctrl |= SmartDrive_CONTROL_GO;
|
||||
if ( motor_id != SmartDrive_Motor_ID_2) {
|
||||
uint8_t array[9] = {SmartDrive_SETPT_M1, t1, t2, t3, t4, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_1){
|
||||
uint8_t array[9] = {SmartDrive_SETPT_M2, t1, t2, t3, t4, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id == SmartDrive_Motor_ID_BOTH )
|
||||
writeByte(SmartDrive_COMMAND, CMD_S);
|
||||
if ( wait_for_completion ) {
|
||||
sleep(1);//this delay is required for the status byte to be available for reading.
|
||||
WaitUntilTachoDone(motor_id);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::Run_Rotations(int motor_id, int direction, uint8_t speed, uint32_t rotations, bool wait_for_completion, int next_action) {
|
||||
uint8_t ctrl = 0;
|
||||
ctrl |= SmartDrive_CONTROL_SPEED;
|
||||
ctrl |= SmartDrive_CONTROL_TACHO;
|
||||
ctrl |= SmartDrive_CONTROL_RELATIVE;
|
||||
|
||||
uint32_t d = rotations * 360;
|
||||
if ( direction != SmartDrive_Dir_Forward )
|
||||
d = (rotations * 360) * -1;
|
||||
|
||||
uint8_t t4 = (d/0x1000000);
|
||||
uint8_t t3 = ((d%0x1000000)/0x10000);
|
||||
uint8_t t2 = (((d%0x1000000)%0x10000)/0x100);
|
||||
uint8_t t1 = (((d%0x1000000)%0x10000)%0x100);
|
||||
|
||||
if ( next_action ==SmartDrive_Action_Brake )
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
if ( next_action ==SmartDrive_Action_BrakeHold ) {
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
ctrl |= SmartDrive_CONTROL_ON;
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_BOTH )
|
||||
ctrl |= SmartDrive_CONTROL_GO;
|
||||
if ( motor_id != SmartDrive_Motor_ID_2) {
|
||||
uint8_t array[9] = {SmartDrive_SETPT_M1, t1, t2, t3, t4, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_1) {
|
||||
uint8_t array[9] = {SmartDrive_SETPT_M2, t1, t2, t3, t4, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id == SmartDrive_Motor_ID_BOTH )
|
||||
writeByte(SmartDrive_COMMAND, CMD_S);
|
||||
if ( wait_for_completion) {
|
||||
sleep(1); //this delay is required for the status byte to be available for reading.
|
||||
WaitUntilTachoDone(motor_id);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::Run_Tacho(int motor_id, uint8_t speed, uint32_t tacho_count, bool wait_for_completion, int next_action) {
|
||||
uint8_t ctrl = 0;
|
||||
ctrl |= SmartDrive_CONTROL_SPEED;
|
||||
ctrl |= SmartDrive_CONTROL_TACHO;
|
||||
|
||||
uint32_t d = tacho_count;
|
||||
|
||||
uint8_t t4 = (d/0x1000000);
|
||||
uint8_t t3 = ((d%0x1000000)/0x10000);
|
||||
uint8_t t2 = (((d%0x1000000)%0x10000)/0x100);
|
||||
uint8_t t1 = (((d%0x1000000)%0x10000)%0x100);
|
||||
|
||||
if ( next_action ==SmartDrive_Action_Brake )
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
if ( next_action ==SmartDrive_Action_BrakeHold ) {
|
||||
ctrl |= SmartDrive_CONTROL_BRK;
|
||||
ctrl |= SmartDrive_CONTROL_ON;
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_BOTH )
|
||||
ctrl |= SmartDrive_CONTROL_GO;
|
||||
if ( motor_id != SmartDrive_Motor_ID_2){
|
||||
uint8_t array[9]= {SmartDrive_SETPT_M1, t1, t2, t3, t4, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id != SmartDrive_Motor_ID_1){
|
||||
uint8_t array[9]= {SmartDrive_SETPT_M2, t1, t2, t3, t4, speed, 0, 0, ctrl};
|
||||
writeArray(array);
|
||||
}
|
||||
if ( motor_id == SmartDrive_Motor_ID_BOTH )
|
||||
writeByte(SmartDrive_COMMAND, CMD_S);
|
||||
if ( wait_for_completion )
|
||||
sleep(1); //this delay is required for the status byte to be available for reading.
|
||||
WaitUntilTachoDone(motor_id);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::WaitUntilTachoDone(int motor_id) {
|
||||
while (IsTachoDone(motor_id) == false)
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
SmartDrive::IsTachoDone(int motor_id) {
|
||||
uint8_t result_1 = 0, result_2 = 0;
|
||||
|
||||
if ( motor_id != SmartDrive_Motor_ID_2 )
|
||||
result_1 = readByte(SmartDrive_STATUS_M1);
|
||||
if ( motor_id != SmartDrive_Motor_ID_1 )
|
||||
result_2 = readByte(SmartDrive_STATUS_M2);
|
||||
//look for both time bits to be zero
|
||||
return (((result_1 & 0x08) == 0) && ((result_2 & 0x08) == 0) );
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::SetPerformanceParameters( uint16_t Kp_tacho, uint16_t Ki_tacho, uint16_t Kd_tacho, uint16_t Kp_speed, uint16_t Ki_speed, uint16_t Kd_speed, uint8_t passcount, uint8_t tolerance) {
|
||||
uint8_t Kp_t1 = Kp_tacho%0x100;
|
||||
uint8_t Kp_t2 = Kp_tacho/0x100;
|
||||
uint8_t Ki_t1 = Ki_tacho%0x100;
|
||||
uint8_t Ki_t2 = Ki_tacho/0x100;
|
||||
uint8_t Kd_t1 = Kd_tacho%0x100;
|
||||
uint8_t Kd_t2 = Kd_tacho/0x100;
|
||||
uint8_t Kp_s1 = Kp_speed%0x100;
|
||||
uint8_t Kp_s2 = Kp_speed/0x100;
|
||||
uint8_t Ki_s1 = Ki_speed%0x100;
|
||||
uint8_t Ki_s2 = Ki_speed/0x100;
|
||||
uint8_t Kd_s1 = Kd_speed%0x100;
|
||||
uint8_t Kd_s2 = Kd_speed/0x100;
|
||||
|
||||
uint8_t array[15] = {SmartDrive_P_Kp, Kp_t1 , Kp_t2 , Ki_t1, Ki_t2, Kd_t1, Kd_t2, Kp_s1, Kp_s2, Ki_s1, Ki_s2, Kd_s1, Kd_s2, passcount, tolerance};
|
||||
writeArray(array);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
SmartDrive::ReadPerformanceParameters() {
|
||||
try {
|
||||
std::cout << "Pkp: " << readInteger(SmartDrive_P_Kp) << std::endl;
|
||||
std::cout << "Pki: " << readInteger(SmartDrive_P_Ki) << std::endl;
|
||||
std::cout << "Pkd: " << readInteger(SmartDrive_P_Kd) << std::endl;
|
||||
std::cout << "Skp: " << readInteger(SmartDrive_S_Kp) << std::endl;
|
||||
std::cout << "Ski: " << readInteger(SmartDrive_S_Ki) << std::endl;
|
||||
std::cout << "Skd: " << readInteger(SmartDrive_S_Kd) << std::endl;
|
||||
std::cout << "Passcount: " << SmartDrive_PASSCOUNT << std::endl; //ToDo : Check if these should actually be Register Reads !!
|
||||
std::cout << "Tolerance: " << SmartDrive_PASSTOLERANCE << std::endl;
|
||||
} catch( int e) {
|
||||
std::cout << "Error: Could not read PID values -> " << e << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t
|
||||
SmartDrive::GetMotorStatus(int motor_id) {
|
||||
uint8_t status=0;
|
||||
if (motor_id == SmartDrive_Motor_ID_1)
|
||||
status = readByte(SmartDrive_STATUS_M1);
|
||||
if (motor_id == SmartDrive_Motor_ID_2)
|
||||
status = readByte(SmartDrive_STATUS_M1);
|
||||
if (motor_id == SmartDrive_Motor_ID_BOTH) {
|
||||
std::cout << "Please specifiy which motor's status you want to fetch !" << std::endl;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
void
|
||||
SmartDrive::PrintMotorStatus(int motor_id) {
|
||||
if (motor_id != SmartDrive_Motor_ID_BOTH) {
|
||||
uint8_t status = GetMotorStatus(motor_id);
|
||||
uint8_t control_on = (status & SmartDrive_MOTOR_CONTROL_ON);
|
||||
uint8_t is_ramping = (status & SmartDrive_MOTOR_IS_RAMPING);
|
||||
uint8_t is_powered = (status & SmartDrive_MOTOR_IS_POWERED);
|
||||
uint8_t pos_control_on = (status & SmartDrive_MOTOR_POS_CTRL_ON);
|
||||
uint8_t in_brake_mode = (status & SmartDrive_MOTOR_IN_BRAKE_MODE);
|
||||
uint8_t is_overloaded = (status & SmartDrive_MOTOR_OVERLOADED);
|
||||
uint8_t in_time_mode = (status & SmartDrive_MOTOR_IN_TIME_MODE);
|
||||
uint8_t is_stalled = (status & SmartDrive_MOTOR_IS_STALLED);
|
||||
|
||||
std::cout << "Motor " << motor_id+1 << " is programemd to move at " << ( (control_on == 0) ? "variable" : "fixed") << " speed" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is " << ((is_ramping == 0) ? "NOT" : "") << " ramping" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is " << ((is_powered == 0) ? "NOT" : "") << " powered" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is " << ((pos_control_on == 0) ? "moving towards desired encoder " : "holding it ") << "position" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is in " << ((in_brake_mode == 0) ? "brake" : "float") << " mode" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is " << ((is_overloaded == 0) ? "NOT" : "") << " overloaded" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is " << ((in_time_mode == 0) ? "NOT" : "") << " in time mode" << std::endl;
|
||||
std::cout << "Motor " << motor_id+1 << " is " << ((is_stalled == 0) ? "NOT" : "") << " stalled" << std::endl;
|
||||
|
||||
} else {
|
||||
std::cout << "Please specifiy which motor's status you want to fetch !" << std::endl;
|
||||
}
|
||||
}
|
||||
303
src/smartdrive/smartdrive.h
Normal file
303
src/smartdrive/smartdrive.h
Normal file
@ -0,0 +1,303 @@
|
||||
/*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Author: Oussema Harbi <oussema.elharbi@gmail.com>
|
||||
* Copyright (c) <2016> <Oussema Harbi>
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||||
* this software and associated documentation files (the "Software"), to deal in
|
||||
* the Software without restriction, including without limitation the rights to
|
||||
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
|
||||
* the Software, and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
|
||||
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
|
||||
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
|
||||
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
|
||||
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <mraa/i2c.hpp>
|
||||
|
||||
//We can use direct integer IDs,
|
||||
//or we can use the typedef here to help limit the error cases
|
||||
//and still support extension to support more Motors in the future
|
||||
//But when using typedef, we need to cast these when sending them in i2c bus
|
||||
#define SmartDrive_Motor_ID_1 0x01
|
||||
#define SmartDrive_Motor_ID_2 0x02
|
||||
#define SmartDrive_Motor_ID_BOTH 0x03
|
||||
|
||||
#define SmartDrive_Dir_Reverse 0x00
|
||||
#define SmartDrive_Dir_Forward 0x01
|
||||
|
||||
#define SmartDrive_Action_Float 0x00 //stop and let the motor coast.
|
||||
#define SmartDrive_Action_Brake 0x01 //apply brakes, and resist change to tachometer
|
||||
#define SmartDrive_Action_BrakeHold 0x02 //apply brakes, and restore externally forced change to tachometer
|
||||
|
||||
//Next action (upon completion of current action)
|
||||
#define SmartDrive_Completion_Wait_For 0x01
|
||||
#define SmartDrive_Completion_Dont_Wait 0x00
|
||||
|
||||
#define SmartDrive_DefaultAddress 0x1b
|
||||
#define SmartDrive_VOLTAGE_MULTIPLIER 212.7
|
||||
|
||||
//Commonly used speed constants, these are just convenience constants
|
||||
//You can use any value between 0 and 100.
|
||||
#define SmartDrive_Speed_Full 90
|
||||
#define SmartDrive_Speed_Medium 60
|
||||
#define SmartDrive_Speed_Slow 25
|
||||
|
||||
//Different commands
|
||||
#define SmartDrive_CONTROL_SPEED 0x01
|
||||
#define SmartDrive_CONTROL_RAMP 0x02
|
||||
#define SmartDrive_CONTROL_RELATIVE 0x04
|
||||
#define SmartDrive_CONTROL_TACHO 0x08
|
||||
#define SmartDrive_CONTROL_BRK 0x10
|
||||
#define SmartDrive_CONTROL_ON 0x20
|
||||
#define SmartDrive_CONTROL_TIME 0x40
|
||||
#define SmartDrive_CONTROL_GO 0x80
|
||||
|
||||
#define SmartDrive_COMMAND 0x41
|
||||
#define SmartDrive_SETPT_M1 0x42
|
||||
#define SmartDrive_SPEED_M1 0x46
|
||||
#define SmartDrive_TIME_M1 0x47
|
||||
#define SmartDrive_CMD_B_M1 0x48
|
||||
#define SmartDrive_CMD_A_M1 0x49
|
||||
|
||||
#define SmartDrive_SETPT_M2 0x4A
|
||||
#define SmartDrive_SPEED_M2 0x4E
|
||||
#define SmartDrive_TIME_M2 0x4F
|
||||
#define SmartDrive_CMD_B_M2 0x50
|
||||
#define SmartDrive_CMD_A_M2 0x51
|
||||
|
||||
//Read registers.
|
||||
#define SmartDrive_POSITION_M1 0x52
|
||||
#define SmartDrive_POSITION_M2 0x56
|
||||
#define SmartDrive_STATUS_M1 0x5A
|
||||
#define SmartDrive_STATUS_M2 0x5B
|
||||
#define SmartDrive_TASKS_M1 0x5C
|
||||
#define SmartDrive_TASKS_M2 0x5D
|
||||
|
||||
//PID control registers
|
||||
#define SmartDrive_P_Kp 0x5E //proportional gain-position
|
||||
#define SmartDrive_P_Ki 0x60 //integral gain-position
|
||||
#define SmartDrive_P_Kd 0x62 //derivative gain-position
|
||||
#define SmartDrive_S_Kp 0x64 //proportional gain-speed
|
||||
#define SmartDrive_S_Ki 0x66 //integral gain-speed
|
||||
#define SmartDrive_S_Kd 0x68 //derivative gain-speed
|
||||
#define SmartDrive_PASSCOUNT 0x6A
|
||||
#define SmartDrive_PASSTOLERANCE 0x6B
|
||||
|
||||
#define SmartDrive_CHKSUM 0x6C
|
||||
|
||||
//Power data registers
|
||||
#define SmartDrive_BATT_VOLTAGE 0x6E
|
||||
#define SmartDrive_RESETSTATUS 0x6F
|
||||
#define SmartDrive_CURRENT_M1 0x70
|
||||
#define SmartDrive_CURRENT_M2 0x72
|
||||
|
||||
//Supported I2C commands
|
||||
#define CMD_R 0x52
|
||||
#define CMD_S 0x53
|
||||
#define CMD_a 0x61
|
||||
#define CMD_b 0x62
|
||||
#define CMD_c 0x63
|
||||
#define CMD_A 0x41
|
||||
#define CMD_B 0x42
|
||||
#define CMD_C 0x43
|
||||
|
||||
//Motor Status Masks
|
||||
#define SmartDrive_MOTOR_CONTROL_ON 0x1
|
||||
#define SmartDrive_MOTOR_IS_RAMPING 0x2
|
||||
#define SmartDrive_MOTOR_IS_POWERED 0x4
|
||||
#define SmartDrive_MOTOR_POS_CTRL_ON 0x8
|
||||
#define SmartDrive_MOTOR_IN_BRAKE_MODE 0x10
|
||||
#define SmartDrive_MOTOR_OVERLOADED 0x20
|
||||
#define SmartDrive_MOTOR_IN_TIME_MODE 0x40
|
||||
#define SmartDrive_MOTOR_IS_STALLED 0x80
|
||||
|
||||
namespace upm {
|
||||
|
||||
/**
|
||||
* @brief SmartDrive library
|
||||
* @defgroup smartdrive libupm-smartdrive
|
||||
* @ingroup i2c motor openelectrons
|
||||
*/
|
||||
|
||||
/**
|
||||
* @library smartdrive
|
||||
* @sensor smartdrive
|
||||
* @comname SmartDrive advanced motor controller
|
||||
* @altname smartdrive
|
||||
* @type motor
|
||||
* @man openelectrons
|
||||
* @con i2c
|
||||
*
|
||||
* @brief API for the SmartDrive advanced motor controller from OpenElectronis
|
||||
*
|
||||
* SmartDrive is a multiplexer to control high current DC motors
|
||||
*
|
||||
* This module has been tested on the SmartDrive.
|
||||
*
|
||||
* @image html smartdrive.jpeg
|
||||
* @snippet smartdrive.cxx Interesting
|
||||
*/
|
||||
//Class definition
|
||||
class SmartDrive {
|
||||
|
||||
public:
|
||||
/**
|
||||
* Initialize the class with the i2c address of your SmartDrive
|
||||
* @param SmartDrive_address Address of your SmartDrive.
|
||||
*/
|
||||
SmartDrive(int i2c_bus, int address = SmartDrive_DefaultAddress);
|
||||
|
||||
/**
|
||||
* Writes a specified command on the command register of the SmartDrive
|
||||
* @param cmd The command you wish the SmartDrive to execute.
|
||||
*/
|
||||
void command(uint8_t cmd);
|
||||
|
||||
/**
|
||||
* Reads the battery voltage. Multiplier constant not yet verified
|
||||
*/
|
||||
float GetBattVoltage();
|
||||
|
||||
/**
|
||||
* Reads the tacheometer position of the specified motor
|
||||
* @param motor_id Number of the motor you wish to read.
|
||||
*/
|
||||
uint32_t ReadTachometerPosition(int motor_id);
|
||||
|
||||
/**
|
||||
* Turns the specified motor(s) forever
|
||||
* @param motor_id Number of the motor(s) you wish to turn.
|
||||
* @param direction The direction you wish to turn the motor(s).
|
||||
* @param speed The speed at which you wish to turn the motor(s).
|
||||
*/
|
||||
void Run_Unlimited(int motor_id, int direction, uint8_t speed);
|
||||
|
||||
/**
|
||||
* Stops the specified motor(s)
|
||||
* @param motor_id Number of the motor(s) you wish to turn.
|
||||
* @param next_action How you wish to stop the motor(s).
|
||||
*/
|
||||
void StopMotor(int motor_id, int next_action );
|
||||
|
||||
/**
|
||||
* Turns the specified motor(s) for a given amount of seconds
|
||||
* @param motor_id Number of the motor(s) you wish to turn.
|
||||
* @param direction The direction you wish to turn the motor(s).
|
||||
* @param speed The speed at which you wish to turn the motor(s).
|
||||
* @param duration The time in seconds you wish to turn the motor(s).
|
||||
* @param wait_for_completion Tells the program when to handle the next line of code.
|
||||
* @param next_action How you wish to stop the motor(s).
|
||||
*/
|
||||
void Run_Seconds(int motor_id, int direction, uint8_t speed, uint8_t duration, bool wait_for_completion, int next_action );
|
||||
|
||||
/**
|
||||
* Waits until the specified time for the motor(s) to run is completed
|
||||
* @param motor_id Number of the motor(s) to wait for.
|
||||
*/
|
||||
void WaitUntilTimeDone(int motor_id);
|
||||
|
||||
/**
|
||||
* Checks to ensure the specified time for the motor(s) to run is completed.
|
||||
* @param motor_id Number of the motor(s) to check.
|
||||
*/
|
||||
bool IsTimeDone(int motor_id);
|
||||
|
||||
/**
|
||||
* Turns the specified motor(s) for given relative tacheometer count
|
||||
* @param motor_id Number of the motor(s) you wish to turn.
|
||||
* @param direction The direction you wish to turn the motor(s).
|
||||
* @param speed The speed at which you wish to turn the motor(s).
|
||||
* @param degrees The relative tacheometer count you wish to turn the motor(s).
|
||||
* @param wait_for_completion Tells the program when to handle the next line of code.
|
||||
* @param next_action How you wish to stop the motor(s).
|
||||
*/
|
||||
void Run_Degrees(int motor_id, int direction, uint8_t speed, uint32_t degrees, bool wait_for_completion, int next_action);
|
||||
|
||||
/**
|
||||
* Turns the specified motor(s) for given relative tacheometer count
|
||||
* @param motor_id Number of the motor(s) you wish to turn.
|
||||
* @param direction The direction you wish to turn the motor(s).
|
||||
* @param speed The speed at which you wish to turn the motor(s).
|
||||
* @param rotations The relative amount of rotations you wish to turn the motor(s).
|
||||
* @param wait_for_completion Tells the program when to handle the next line of code.
|
||||
* @param next_action How you wish to stop the motor(s).
|
||||
*/
|
||||
void Run_Rotations(int motor_id, int direction, uint8_t speed, uint32_t rotations, bool wait_for_completion, int next_action);
|
||||
|
||||
/**
|
||||
* Turns the specified motor(s) for given absolute tacheometer count
|
||||
* @param motor_id Number of the motor(s) you wish to turn.
|
||||
* @param direction The direction you wish to turn the motor(s).
|
||||
* @param speed The speed at which you wish to turn the motor(s).
|
||||
* @param tacho_count The absolute tacheometer count you wish to turn the motor(s).
|
||||
* @param wait_for_completion Tells the program when to handle the next line of code.
|
||||
* @param next_action How you wish to stop the motor(s).
|
||||
*/
|
||||
void Run_Tacho(int motor_id, uint8_t speed, uint32_t tacho_count, bool wait_for_completion, int next_action);
|
||||
|
||||
/**
|
||||
* Waits until the specified tacheomter count for the motor(s) to run is reached.
|
||||
* @param motor_id Number of the motor(s) to wait for.
|
||||
*/
|
||||
void WaitUntilTachoDone(int motor_id);
|
||||
|
||||
/**
|
||||
* Checks to ensure the specified tacheomter count for the motor(s) to run is reached.
|
||||
* @param motor_id Number of the motor(s) to check.
|
||||
*/
|
||||
bool IsTachoDone(int motor_id);
|
||||
|
||||
/**
|
||||
* Writes user specified values to the PID control registers
|
||||
* @param Kp_tacho Proportional-gain of the tacheometer position of the motor.
|
||||
* @param Ki_tacho Integral-gain of the tacheometer position of the motor.
|
||||
* @param Kd_tacho Derivative-gain of the tacheometer position of the motor.
|
||||
* @param Kp_speed Proportional-gain of the speed of the motor.
|
||||
* @param Ki_speed Integral-gain of the speed of the motor.
|
||||
* @param Kd_speed Derivative-gain of the speed of the motor.
|
||||
*/
|
||||
void SetPerformanceParameters( uint16_t Kp_tacho, uint16_t Ki_tacho, uint16_t Kd_tacho, uint16_t Kp_speed, uint16_t Ki_speed, uint16_t Kd_speed, uint8_t passcount, uint8_t tolerance);
|
||||
|
||||
/**
|
||||
* Reads the values of the PID control registers
|
||||
*/
|
||||
void ReadPerformanceParameters();
|
||||
|
||||
/**
|
||||
* Read the status of a motor, and return it in a uint8_t
|
||||
* param motor_id Number fo the motor to check
|
||||
*/
|
||||
uint8_t GetMotorStatus(int motor_id);
|
||||
|
||||
/**
|
||||
* Print the detailed status of the motor
|
||||
* @param motor_id Number fo the motor to check
|
||||
*/
|
||||
void PrintMotorStatus(int motor_id);
|
||||
|
||||
private:
|
||||
void writeByte(uint8_t addr, uint8_t value);
|
||||
void writeArray(uint8_t* array);
|
||||
uint8_t readByte(uint8_t addr);
|
||||
uint16_t readInteger(uint8_t addr);
|
||||
uint32_t readLongSigned(uint8_t addr);
|
||||
|
||||
private:
|
||||
int m_smartdrive_control_address;
|
||||
mraa::I2c m_i2c_smartdrive_control;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user