diff --git a/Software/C/CMakeLists.txt b/Software/C/CMakeLists.txt index 27ce861..80d40ee 100644 --- a/Software/C/CMakeLists.txt +++ b/Software/C/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0) project(gopigo3_cpp) ### Build @@ -38,6 +38,14 @@ target_link_libraries(sensors gopigo3) add_executable(drive Examples/drive.cpp) target_link_libraries(drive gopigo3) +# servo example +add_executable(servos Examples/servos.cpp) +target_link_libraries(servos gopigo3) + +# vbatt example +add_executable(vbatt Examples/vbatt.cpp) +target_link_libraries(vbatt gopigo3) + ### Installation install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/ diff --git a/Software/C/Examples/drive.cpp b/Software/C/Examples/drive.cpp index 03bda1a..688702c 100644 --- a/Software/C/Examples/drive.cpp +++ b/Software/C/Examples/drive.cpp @@ -62,10 +62,9 @@ int main(){ GPG.reset_motor_encoder(MOTOR_LEFT + MOTOR_RIGHT); - // GPG.set_motor_limits(MOTOR_LEFT,0,300); - // GPG.set_motor_limits(MOTOR_RIGHT,0,300); - GPG.set_motor_limits(MOTOR_LEFT,0,0); - GPG.set_motor_limits(MOTOR_RIGHT,0,0); + // set limits to safe value that will not allow GoPiGo3 to tip over + GPG.set_motor_limits(MOTOR_LEFT,0,150); + GPG.set_motor_limits(MOTOR_RIGHT,0,150); do{ @@ -83,6 +82,7 @@ int main(){ switch(c){ case 'w': // forward GPG.set_motor_dps(MOTOR_LEFT + MOTOR_RIGHT, NO_LIMIT_SPEED); + // GPG.set_motor_power(MOTOR_LEFT + MOTOR_RIGHT, 10); break; case 'x' : // backward GPG.set_motor_dps(MOTOR_LEFT + MOTOR_RIGHT, NO_LIMIT_SPEED * -1); diff --git a/Software/C/Examples/servos.cpp b/Software/C/Examples/servos.cpp new file mode 100644 index 0000000..510468e --- /dev/null +++ b/Software/C/Examples/servos.cpp @@ -0,0 +1,54 @@ +/* + * https://www.dexterindustries.com/GoPiGo3/ + * https://github.com/DexterInd/GoPiGo3 + * + * Copyright (c) 2017 Dexter Industries + * Released under the MIT license (http://choosealicense.com/licenses/mit/). + * For more information see https://github.com/DexterInd/GoPiGo3/blob/master/LICENSE.md + * + * This code is an example for using GoPiGo3 servos. + * + * Results: When you run this program, you should see the servos center. + * + * To compile see: https://github.com/slowrunner/Carl/blob/master/Examples/cpp/Alan_Note.txt + */ + +#include // for GoPiGo3 +#include // for printf +#include // for usleep +#include // for catching exit signals + +GoPiGo3 GPG; + +void exit_signal_handler(int signo); + +int main(){ + signal(SIGINT, exit_signal_handler); // register the exit function for Ctrl+C + + GPG.detect(); // Make sure that the GoPiGo3 is communicating and that the firmware is compatible with the drivers. + + printf("\nReseting servos to center\n"); + + // Reset the servos to center + GPG.set_servo(SERVO_1,1500); + // let one servo move at a time to keep load smaller + usleep(200000); + GPG.set_servo(SERVO_2,1500); + usleep(200000); + // Turn servo power off + GPG.set_servo(SERVO_1,0); + GPG.set_servo(SERVO_2,0); + + // while(true){ + // usleep(2000); + // } + GPG.reset_all(); +} + +// Signal handler that will be called when Ctrl+C is pressed to stop the program +void exit_signal_handler(int signo){ + if(signo == SIGINT){ + GPG.reset_all(); // Reset everything so there are no run-away motors + exit(-2); + } +} diff --git a/Software/C/Examples/vbatt.cpp b/Software/C/Examples/vbatt.cpp new file mode 100644 index 0000000..f56af43 --- /dev/null +++ b/Software/C/Examples/vbatt.cpp @@ -0,0 +1,23 @@ +/* + * This code is an example for reading GoPiGo3 battery voltage + * + * Results: Prints float battery voltage + * + */ + +#include // for GoPiGo3 +#include // for printf + +GoPiGo3 GPG; // Create a GoPiGo3 instance + +int main(){ + + // Make sure that the GoPiGo3 is communicating and that the firmware is compatible with the drivers. + // pass 'false' to detect() to make the error non-critical (return the error instead of exiting the program). + if(GPG.detect(false) == ERROR_NONE){ + printf(" Battery voltage : %.3f\n", GPG.get_voltage_battery()); + printf(" 5v voltage : %.3f\n", GPG.get_voltage_5v()); + }else{ + printf("\nError communicating with GoPiGo3\n"); + } +} diff --git a/Software/C/GoPiGo3.cpp b/Software/C/GoPiGo3.cpp index 7d99428..b14642d 100644 --- a/Software/C/GoPiGo3.cpp +++ b/Software/C/GoPiGo3.cpp @@ -443,6 +443,7 @@ int GoPiGo3::set_motor_position(uint8_t port, int32_t position){ spi_array_out[0] = Address; spi_array_out[1] = GPGSPI_MESSAGE_SET_MOTOR_POSITION; spi_array_out[2] = port; + position = (int) (position * MOTOR_TICKS_PER_DEGREE); spi_array_out[3] = ((position >> 24) & 0xFF); spi_array_out[4] = ((position >> 16) & 0xFF); spi_array_out[5] = ((position >> 8) & 0xFF); @@ -454,6 +455,7 @@ int GoPiGo3::set_motor_dps(uint8_t port, int16_t dps){ spi_array_out[0] = Address; spi_array_out[1] = GPGSPI_MESSAGE_SET_MOTOR_DPS; spi_array_out[2] = port; + dps = (int)(dps * MOTOR_TICKS_PER_DEGREE); spi_array_out[3] = ((dps >> 8) & 0xFF); spi_array_out[4] = (dps & 0xFF); return spi_transfer_array(5, spi_array_out, spi_array_in); @@ -464,6 +466,7 @@ int GoPiGo3::set_motor_limits(uint8_t port, uint8_t power, uint16_t dps){ spi_array_out[1] = GPGSPI_MESSAGE_SET_MOTOR_LIMITS; spi_array_out[2] = port; spi_array_out[3] = power; + dps = (int)(dps * MOTOR_TICKS_PER_DEGREE); spi_array_out[4] = ((dps >> 8) & 0xFF); spi_array_out[5] = (dps & 0xFF); return spi_transfer_array(6, spi_array_out, spi_array_in); @@ -495,8 +498,9 @@ int GoPiGo3::get_motor_status(uint8_t port, uint8_t &state, int8_t &power, int32 state = spi_array_in[4]; power = spi_array_in[5]; position = ((spi_array_in[6] << 24) | (spi_array_in[7] << 16) | (spi_array_in[8] << 8) | spi_array_in[9]); + position = (int)(position / MOTOR_TICKS_PER_DEGREE); dps = ((spi_array_in[10] << 8) | spi_array_in[11]); - + dps = (int)(dps / MOTOR_TICKS_PER_DEGREE); return ERROR_NONE; } @@ -504,6 +508,7 @@ int GoPiGo3::offset_motor_encoder(uint8_t port, int32_t position){ spi_array_out[0] = Address; spi_array_out[1] = GPGSPI_MESSAGE_OFFSET_MOTOR_ENCODER; spi_array_out[2] = port; + position = (int)(position * MOTOR_TICKS_PER_DEGREE); spi_array_out[3] = ((position >> 24) & 0xFF); spi_array_out[4] = ((position >> 16) & 0xFF); spi_array_out[5] = ((position >> 8) & 0xFF); @@ -513,7 +518,7 @@ int GoPiGo3::offset_motor_encoder(uint8_t port, int32_t position){ int32_t GoPiGo3::get_motor_encoder(uint8_t port){ int32_t value; - get_motor_encoder(port, value); + get_motor_encoder(port, value); // value returned is corrected for MOTOR_TICKS_PER_DEGREE return value; } @@ -542,7 +547,7 @@ int GoPiGo3::get_motor_encoder(uint8_t port, int32_t &value){ } uint32_t Value; int res = spi_read_32(msg_type, Value); - value = Value; + value = (int)(Value / MOTOR_TICKS_PER_DEGREE); return res; } diff --git a/Software/C/package.xml b/Software/C/package.xml index b4f4113..35caf08 100644 --- a/Software/C/package.xml +++ b/Software/C/package.xml @@ -1,7 +1,7 @@ gopigo3_cpp - 0.0.1 + 0.0.2 C++ driver for GoPiGo3 Modular Robotics