From 7e402ede3495dff5e2a6076d5f59484687013fe6 Mon Sep 17 00:00:00 2001 From: Oussema Harbi Date: Wed, 16 Mar 2016 21:24:11 +0100 Subject: [PATCH] smartdrive: upm implementation for SmartDrive from openelectronics.com Signed-off-by: Oussema Harbi Signed-off-by: Mihai Tudor Panu --- examples/c++/smartdrive.cxx | 73 +++++ src/smartdrive/CMakeLists.txt | 5 + src/smartdrive/javaupm_smartdrive.i | 8 + src/smartdrive/jsupm_smartdrive.i | 8 + src/smartdrive/pyupm_smartdrive.i | 9 + src/smartdrive/smartdrive.cxx | 430 ++++++++++++++++++++++++++++ src/smartdrive/smartdrive.h | 303 ++++++++++++++++++++ 7 files changed, 836 insertions(+) create mode 100644 examples/c++/smartdrive.cxx create mode 100644 src/smartdrive/CMakeLists.txt create mode 100644 src/smartdrive/javaupm_smartdrive.i create mode 100644 src/smartdrive/jsupm_smartdrive.i create mode 100644 src/smartdrive/pyupm_smartdrive.i create mode 100644 src/smartdrive/smartdrive.cxx create mode 100644 src/smartdrive/smartdrive.h diff --git a/examples/c++/smartdrive.cxx b/examples/c++/smartdrive.cxx new file mode 100644 index 00000000..992f88aa --- /dev/null +++ b/examples/c++/smartdrive.cxx @@ -0,0 +1,73 @@ +/* + * The MIT License (MIT) + * + * Author: Oussema Harbi + * Copyright (c) <2016> + * + * 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 +#include +#include "smartdrive.h" +#include + +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; +} diff --git a/src/smartdrive/CMakeLists.txt b/src/smartdrive/CMakeLists.txt new file mode 100644 index 00000000..3c9b0d22 --- /dev/null +++ b/src/smartdrive/CMakeLists.txt @@ -0,0 +1,5 @@ +set (libname "smartdrive") +set (libdescription "upm SmartDrive") +set (module_src ${libname}.cxx) +set (module_h ${libname}.h) +upm_module_init() diff --git a/src/smartdrive/javaupm_smartdrive.i b/src/smartdrive/javaupm_smartdrive.i new file mode 100644 index 00000000..438d45b5 --- /dev/null +++ b/src/smartdrive/javaupm_smartdrive.i @@ -0,0 +1,8 @@ +%module javaupm_smartdrive +%include "../upm.i" + +%{ + #include "smartdrive.h" +%} + +%include "smartdrive.h" diff --git a/src/smartdrive/jsupm_smartdrive.i b/src/smartdrive/jsupm_smartdrive.i new file mode 100644 index 00000000..cd81c983 --- /dev/null +++ b/src/smartdrive/jsupm_smartdrive.i @@ -0,0 +1,8 @@ +%module jsupm_smartdrive +%include "../upm.i" + +%{ + #include "smartdrive.h" +%} + +%include "smartdrive.h" diff --git a/src/smartdrive/pyupm_smartdrive.i b/src/smartdrive/pyupm_smartdrive.i new file mode 100644 index 00000000..2bb84024 --- /dev/null +++ b/src/smartdrive/pyupm_smartdrive.i @@ -0,0 +1,9 @@ +%module pyupm_smartdrive +%include "../upm.i" + +%include "stdint.i" + +%include "smartdrive.h" +%{ + #include "smartdrive.h" +%} diff --git a/src/smartdrive/smartdrive.cxx b/src/smartdrive/smartdrive.cxx new file mode 100644 index 00000000..361592d2 --- /dev/null +++ b/src/smartdrive/smartdrive.cxx @@ -0,0 +1,430 @@ +/* + * The MIT License (MIT) + * + * Author: Oussema Harbi + * Copyright (c) <2016> + * + * 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 +#include +#include +#include +#include +#include + +#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; + } +} diff --git a/src/smartdrive/smartdrive.h b/src/smartdrive/smartdrive.h new file mode 100644 index 00000000..280bac3b --- /dev/null +++ b/src/smartdrive/smartdrive.h @@ -0,0 +1,303 @@ +/* + * The MIT License (MIT) + * + * Author: Oussema Harbi + * Copyright (c) <2016> + * + * 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 + +//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; + +}; + +}