Motors don't work in adding Autonomous toggle to Cockpit UI interface


#1

Hey Folks,

I want to add autonomous toggle switch in cockpit interface. Say I want to execute a particular task when I hit a particular key in the keyboard. For testing I add a simple code for running all motors for 5 seconds and printing something serially when I hit 'hold heading toggle switch'. The issue is I can see the serial output printing on my browser but the motors does not run.

later I thought of running motors on its own when the robot starts (After powering up). For simplicity I added the sweep example of servo from Arduino in the loop of openrov.ino file.

#include "AConfig.h"

#include <Arduino.h>

#include "Motors.h"
#include "Command.h"
#include "Device.h"
#include "Timer.h"
#include "Pin.h"
#include "Settings.h"
#include <avr/wdt.h> // watchdog timer
#include <EEPROM.h>
#include "openrov_servo.h"

byte check = 0;

//Include the defined headers in the settings.h file for the different devices that may be wired in. For
//devices that have pin numbers wired to a board they are added in the cape or controllerboard cpp file because
//we need the pin numbers.

Settings settings;


#if(HAS_STD_CAPE)
#include "Cape.h"
Cape cape;
#endif

#if(HAS_OROV_CONTROLLERBOARD_25)
#include "controllerboard25.h"
Controller25 controller;
#endif

#if(HAS_STD_LIGHTS)
#include "Lights.h"
Lights lights;
#endif

#if(HAS_STD_CALIBRATIONLASERS)
#include "CalibrationLaser.h"
CalibrationLaser calibrationLaser;
#endif

#if(HAS_STD_2X1_THRUSTERS)
#include "Thrusters2X1.h"
Thrusters thrusters;
#endif

#if(HAS_STD_PILOT)
#include "Pilot.h"
Pilot pilot;
#endif

#if(HAS_STD_CAMERAMOUNT)
#include "CameraMount.h"
CameraMount cameramount;
#endif

#if(HAS_POLOLU_MINIMUV)
#define COMPASS_ENABLED 1
#define GYRO_ENABLED 1
#define ACCELEROMETER_ENABLED 1
#include "MinIMU9.h"
#include <Wire.h> //required to force the Arduino IDE to include the library in the path for the I2C code
MinIMU9 IMU;
#endif

#if(HAS_MPU9150)
#define COMPASS_ENABLED 1
#define GYRO_ENABLED 1
#define ACCELEROMETER_ENABLED 1
#include "MPU9150.h"
#include <Wire.h> //required to force the Arduino IDE to include the library in the path for the I2C code

MPU9150 IMU;
#endif

#if(HAS_MS5803_14BA)
#define DEAPTH_ENABLED 1
#include "MS5803_14BA.h"
#include <Wire.h> //required to force the Arduino IDE to include the library in the path for the I2C code
#include <SPI.h> //required to force the Arduino IDE to include the library in the path for the SPI code
MS5803_14BA DeapthSensor;
#endif


Command cmd;

//Pratap Bhanu Code :

Servo myservo1, myservo2, myservo3; // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0; // variable to store the servo position

int loops_counter;
//

volatile byte wdt_resets = 0; //watchdog resets

// IMPORTANT!
// array[0] will be the number of arguments in the command
int array[MAX_ARGS];
Timer Output1000ms;
Timer Output100ms;
int loops_per_sec;


void setup(){
disableWatchdog();
enableWatchdog();
Serial.begin(115200);
loops_counter = 0;
//watchdogOn();
check = EEPROM.read(0);

// if the watchdog triggered and the ISR completed, the first EEPROM byte will be a "1"
if(check == 1)
{
wdt_resets = EEPROM.read(1);
EEPROM.write(0,0); // reset byte so the EEPROM is not read on next startup
Serial.println("log:Watchdog was triggered and the following was read from EEPROM;");
Serial.print("log:");
Serial.println(wdt_resets);
Serial.print(';');
}

pinMode(13, OUTPUT);
Output1000ms.reset();
Output100ms.reset();

DeviceManager::doDeviceSetups();
pinMode(16,OUTPUT);
digitalWrite(16,1);
myservo1.attach(6); // attaches the servo on pin 7 to the servo object
myservo2.attach(7);
myservo3.attach(8);
}


void loop(){
wdt_reset();
cmd.get();
// if( cmd.cmp("set_auto_pilot")){
// play_motors.go(1800, 1800, 1800);
// delay(10000);
// }
pinMode(16,OUTPUT);
digitalWrite(16,1);
myservo1.attach(6); // attaches the servo on pin 7 to the servo object
myservo2.attach(7);
myservo3.attach(8);
for(pos = 0; pos <= 180; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo1.write(pos); // tell servo to go to position in variable 'pos'
myservo2.write(pos);
myservo3.write(pos);
delay(20); // waits 15ms for the servo to reach the position
}
for(pos = 180; pos>=0; pos-=1) // goes from 180 degrees to 0 degrees
{
myservo1.write(pos); // tell servo to go to position in variable 'pos'
myservo2.write(pos);
myservo3.write(pos);
delay(20); // waits 15ms for the servo to reach the position
}
// //*****************************************************//
// }
// else{
// DeviceManager::doDeviceLoops(cmd);
// loops_per_sec++;
// if (Output1000ms.elapsed(1000)) {
// OutputSharedData();
// Serial.print(F("alps:"));
// Serial.print(loops_per_sec);
// Serial.println(';');
// loops_per_sec = 0;
// }
// if (Output100ms.elapsed(100)) {
// OutputNavData();
// }
// }
// loops_counter++;
}


/* sets the watchdog timer both interrupt and reset mode with an 8 second timeout */
void enableWatchdog()
{
cli();
MCUSR &= ~(1<<WDRF);
wdt_reset();
WDTCSR |= (1<<WDCE) | (1<<WDE);
WDTCSR = (~(1<<WDP1) & ~(1<<WDP2)) | ((1<<WDE) | (1<<WDIE) | (1<<WDP3) | (1<<WDP0));
sei();
}

/* disables the watchdog timer */
void disableWatchdog()
{
cli();
wdt_reset();
MCUSR &= ~(1<<WDRF);
WDTCSR |= (1<<WDCE) | (1<<WDE);
WDTCSR = 0x00;
sei();
}

/* this is called when the watchdog times out and before the reset */
ISR(WDT_vect)
{

EEPROM.write(1, wdt_resets+1); // write the random number to the second byte
EEPROM.write(0,1); // write a "1" to the first byte to indicate the data in second byte is valid and the ISR triggered properly
while(true); // triggers the second watchdog timeout for a reset
}

This does not work. The motors still do not run. However when I just add this simple code in openrov.ino. The motors works as they supposed to work: increase speed and then decrease speed.

Working Code:

#include <Servo.h>

Servo myservo1,myservo2, myservo3; // create servo object to control a servo

// twelve servo objects can be created on most boards

int pos = 0; // variable to store the servo position

int loops_counter;

void setup()

{

pinMode(16,OUTPUT);

myservo1.attach(6); // attaches the servo on pin 7 to the servo object

myservo2.attach(7);

myservo3.attach(8);

loops_counter =0;

}

void loop()

{

//*****************************************************//

pinMode(16,OUTPUT);

digitalWrite(16,1);

myservo1.attach(6); // attaches the servo on pin 7 to the servo object

myservo2.attach(7);

myservo3.attach(8);

for(pos = 0; pos <= 180; pos += 1) // goes from 0 degrees to 180 degrees

{ // in steps of 1 degree

myservo1.write(pos); // tell servo to go to position in variable 'pos'

myservo2.write(pos);

myservo3.write(pos);

delay(20); // waits 15ms for the servo to reach the position

}

for(pos = 180; pos>=0; pos-=1) // goes from 180 degrees to 0 degrees

{

myservo1.write(pos); // tell servo to go to position in variable 'pos'

myservo2.write(pos);

myservo3.write(pos);

delay(20); // waits 15ms for the servo to reach the position

}

//*****************************************************//

}

I think when I include different libraries from OpenROV, somehow they suppress the motion of motors or deactivate the motors. Could anyone please help me figure out this issue or give me some insights where I might be doing wrong. I would appreciate any effort for help.


#2

Hey Pratap,

The modules that load grab the servo pins, so your code in the first example fails as a result. When you removed all of the modules, you had exclusive access.

The thrusters library already abstracts away the details behind an API so you can simply pass Thrust and Yaw commands. Take a look at the pilot.cpp for examples.

The intent for extending the libraries is to add your own module based on the others that are there that inherit from the device class, that you would then link in with the following pattern.

#if(HAS_MYMODULE)
#include "MyModule.h"
MyModule CustomModule;
#endif

That allows you to focus on just your code and not worry about it getting trampled by updates. It also keeps things simple since you know the original code has not been touched.

-Brian


#3

Hey Brian,

I really appreciate your early response.

I had figured out that the modules are capturing the pins, but I was assuming that there must be some way around it. If I want to use thruster library like in pilot.cpp and say I want to send a,b,c values as thrust. Could you please tell me a way to use command.pushCommand to do that.

Or suppose I want to make all of the motors go at certain speed a,b,c when I press speedToggle key: say 'Q'. The motors should run at speed corresponds to a,b,c respectively till I hit Q again.

I will put this in my Pilot.cpp

*********************************************

if( command.cmp("speedToggle")){
if (_speedToggleEnabled) {
_speedToggleEnabled = false;
// Code for sending speed values to motors
} else {
_speedToggleEnabled = true;
//do something
}
}

**********************************************************

Could you please look into it and check if my approach is correct or not. Also if my approach is correct could you please give me a code snippet or a hint about how to send a,b,c values to motors in the commented '// Code for sending speed values to motors' part.

Thanks

-Pratap