void MSERNode::tick( double dt )
{
Timer t;
t.start();
Pin* outPin = findPinFromLabel("out");
Pin* inPin = findPinFromLabel("in");
if( !inPin->isConnected() )
waitForConnection();
if( !outPin->isConnected() )
waitForConnection();
if( outPin->isConnected() && inPin->isConnected() )
{
vector<ObjectBlob*> b = inPin->read();
if( b.size() > 0 )
{
for( int k = 0; k < b.size(); ++k )
{
if( b[k]->getTypeName() == "Image" )
{
monadic::Image img;
img.deserialize(b[k]);
cv::Mat m( img.getHeight(), img.getWidth(), CV_8UC3, img.ptr() );
cv::Mat res( img.getHeight(), img.getWidth(), CV_8UC3 );
res = m.clone();
cv::cvtColor( m, m, CV_RGB2GRAY );
vector< vector< cv::Point > > ret;
cv::MSER* det = (cv::MSER*)(detector);
(*det)(m, ret);
for( size_t i = 0; i < ret.size(); ++i )
{
//cv::circle( res, kps[i].pt, 3, CV_RGB(255,0,0) );
cv::Rect r = cv::boundingRect(ret[i]);
cv::rectangle( res, r, CV_RGB(255,0,0), 3 );
}
monadic::Image imgout;
imgout.create( res.cols, res.rows, 8, res.channels() );
size_t bufferSize = res.cols * res.rows * res.channels();
imgout.copyFrom( (char*)res.data, bufferSize );
ObjectBlob* bout = imgout.serialize();
outPin->write( bout );
delete bout;
}
delete b[k];
}
}
}
t.stop();
}
void Thrusters::device_setup(){
port_motor.reset();
vertical_motor.reset();
starboard_motor.reset();
thrusterOutput.reset();
controltime.reset();
bypasssmoothing = false;
#ifdef ESCPOWER_PIN
escpower.reset();
escpower.write(1); //Turn on the ESCs
#endif
}
void Lights::device_loop(Command command){
if( command.cmp("ligt")){
int value = command.args[1]; //0 - 255
light.write(value);
_SERIAL_PORT_.print(F("LIGT:"));
_SERIAL_PORT_.print(value);
_SERIAL_PORT_.print(';');
_SERIAL_PORT_.print(F("LIGP:"));
_SERIAL_PORT_.print(command.args[1]/255.0);
_SERIAL_PORT_.println(';');
}
}
void Lights::device_setup(){
Settings::capability_bitarray |= (1 << LIGHTS_CAPABLE);
light.reset();
light.write(0);
}
void Thrusters::device_loop(Command command){
if (command.cmp("mtrmod")) {
port_motor.motor_positive_modifer = command.args[1]/100;
vertical_motor.motor_positive_modifer = command.args[2]/100;
starboard_motor.motor_positive_modifer = command.args[3]/100;
port_motor.motor_negative_modifer = command.args[4]/100;
vertical_motor.motor_negative_modifer = command.args[5]/100;
starboard_motor.motor_negative_modifer = command.args[6]/100;
}
if (command.cmp("rmtrmod")) {
Serial.print(F("mtrmod:"));
Serial.print(port_motor.motor_positive_modifer);
Serial.print (",");
Serial.print(vertical_motor.motor_positive_modifer);
Serial.print (",");
Serial.print(starboard_motor.motor_positive_modifer);
Serial.print (",");
Serial.print(port_motor.motor_negative_modifer);
Serial.print (",");
Serial.print(vertical_motor.motor_negative_modifer);
Serial.print (",");
Serial.print(starboard_motor.motor_negative_modifer);
Serial.println (";");
}
if (command.cmp("go")) {
//ignore corrupt data
if (command.args[1]>999 && command.args[2] >999 && command.args[3] > 999 && command.args[1]<2001 && command.args[2]<2001 && command.args[3] < 2001) {
p = command.args[1];
v = command.args[2];
s = command.args[3];
if (command.args[4] == 1) bypasssmoothing=true;
}
}
if (command.cmp("port")) {
//ignore corrupt data
if (command.args[1]>999 && command.args[1]<2001) {
p = command.args[1];
if (command.args[2] == 1) bypasssmoothing=true;
}
}
if (command.cmp("vertical")) {
//ignore corrupt data
if (command.args[1]>999 && command.args[1]<2001) {
v = command.args[1];
if (command.args[2] == 1) bypasssmoothing=true;
}
}
if (command.cmp("starboard")) {
//ignore corrupt data
if (command.args[1]>999 && command.args[1]<2001) {
s = command.args[1];
if (command.args[2] == 1) bypasssmoothing=true;
}
}
if (command.cmp("thro") || command.cmp("yaw")){
if (command.cmp("thro")){
if (command.args[1]>=-100 && command.args[1]<=100) {
trg_throttle = command.args[1]/100.0;
}
}
if (command.cmp("yaw")) {
//ignore corrupt data
if (command.args[1]>=-100 && command.args[1]<=100) { //percent of max turn
trg_yaw = command.args[1]/100.0;
}
}
// The code below spreads the throttle spectrum over the possible range
// of the motor. Not sure this belongs here or should be placed with
// deadzon calculation in the motor code.
if (trg_throttle>=0){
p = 1500 + (500/abs(port_motor.motor_positive_modifer))*trg_throttle;
s = p;
} else {
p = 1500 + (500/abs(port_motor.motor_negative_modifer))*trg_throttle;
s = p;
}
trg_motor_power = s;
int turn = trg_yaw*250; //max range due to reverse range
if (trg_throttle >=0){
int offset = (abs(turn)+trg_motor_power)-2000;
if (offset < 0) offset = 0;
p = trg_motor_power+turn-offset;
s = trg_motor_power-turn-offset;
} else {
int offset = 1000-(trg_motor_power-abs(turn));
if (offset < 0) offset = 0;
p = trg_motor_power+turn+offset;
s = trg_motor_power-turn+offset;
}
}
if (command.cmp("lift")){
if (command.args[1]>=-100 && command.args[1]<=100) {
trg_lift = command.args[1]/100.0;
v = 1500 + 500*trg_lift;
}
}
#ifdef ESCPOWER_PIN
else if (command.cmp("escp")) {
escpower.write(command.args[1]); //Turn on the ESCs
Serial.print(F("log:escpower="));
Serial.print(command.args[1]);
Serial.println(';');
}
#endif
else if (command.cmp("start")) {
port_motor.reset();
vertical_motor.reset();
starboard_motor.reset();
}
else if (command.cmp("stop")) {
p = MIDPOINT;
v = MIDPOINT;
s = MIDPOINT;
// Not sure why the reset does not re-attach the servo.
//port_motor.stop();
//vertical_motor.stop();
//starboard_motor.stop();
}
#ifdef ESCPOWER_PIN
else if ((command.cmp("mcal")) && (canPowerESCs)){
Serial.println(F("log:Motor Callibration Staring;"));
//Experimental. Add calibration code here
Serial.println(F("log:Motor Callibration Complete;"));
}
#endif
//to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
//to their new positions in increments. The incriment should eventually be adjustable from the cockpit so that
//the pilot could have more aggressive response profiles for the ROV.
if (controltime.elapsed (50)) {
if (p!=new_p || v!=new_v || s!=new_s) {
new_p = smoothAdjustedServoPosition(p,new_p);
new_v = smoothAdjustedServoPosition(v,new_v);
new_s = smoothAdjustedServoPosition(s,new_s);
if (bypasssmoothing)
{
new_p=p;
new_v=v;
new_s=s;
bypasssmoothing = false;
}
Serial.print(F("motors:"));
Serial.print(port_motor.goms(new_p));
Serial.print(',');
Serial.print(vertical_motor.goms(new_v));
Serial.print(',');
Serial.print(starboard_motor.goms(new_s));
Serial.println(';');
}
}
navdata::FTHR = map((new_p + new_s) / 2, 1000,2000,-100,100);
//The output from the motors is unique to the thruster configuration
if (thrusterOutput.elapsed(1000)){
Serial.print(F("mtarg:"));
Serial.print(p);
Serial.print(',');
Serial.print(v);
Serial.print(',');
Serial.print(s);
Serial.println(';');
thrusterdata::MATC = port_motor.attached() || port_motor.attached() || port_motor.attached();
}
}
