View unanswered posts | View active topics It is currently Fri Apr 19, 2019 6:20 am






Reply to topic  [ 1 post ] 
Code to verify HiTechnic Motor/Servo Controllers 
Author Message
Rookie

Joined: Sat Dec 06, 2014 9:59 am
Posts: 1
Post Code to verify HiTechnic Motor/Servo Controllers
Our FTC team is looking for tips on how to properly verify (via code) that we have the proper HiTechnic motor/servo controllers plugged in. The problem we're trying to solve is twofold: (1) an unnoticed loose sensor wire causes us to not have a physical connection to one or more controllers, and (2) rare behavior we've noticed where either the NXT or motor/servo controllers will not communicate on first start. We think it would be valuable to many teams to have a reliable way to verify that the NXT "sees" the correct set of controllers on program start, and warns the user if it appears incorrect.

We've looked around the documentation, but not found any easy way to verify that the messages received from the controllers (via I2C) indicate that the correct devices are attached. The ideal solution would be to have code that would automatically be able to verify the set of controllers based on the pragma lines in the beginning of our program, though a more manual approach would work as well. Any suggestions would be greatly appreciated.

We have coded up a method to deal with this ourselves (which I'll post below), but since we don't know the internals of robotc and the native I2C communications, we are not confident that the code is safe, especially that it is thread safe with the native I2C communications to the controllers handled by the runtime. We'd appreciate help from any experts in this area!

Here is the library we've been using (the one we're not sure is thread safe), along with test code to use it.

Code:
#pragma config(Hubs,  S2, HTServo,  HTMotor,  none,     none)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,          m1,            tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          m2,            tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          m3,            tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "HTControllerUtil.h"

task main()
{

   // Verify that on sensor port 2 we have a servo controller in position 1 and a motor controller in position 2
   // Note: verifyHTController will hold execution on failure
   verifyHTController(S2,HTServoController,HTDaisyChainPosition1);
   verifyHTController(S2,HTMotorController,HTDaisyChainPosition2);

   nxtDisplayTextLine(4,"Controllers Verified");
}



And here is our library code we could use help with!

Code:
HTController.h

/*

Test for presence of HiTechnic Controllers.  Needs to be cleaned up a bit and documented

*/

#pragma systemFile

// typedefs to make sure we get the expected values
typedef string HTControllerType;
typedef byte HTControllerLoc;

// Controller Types
HTControllerType HTServoController="Servo";
HTControllerType HTMotorController="Motor";

// Controller Locations
const HTControllerLoc HTDaisyChainPosition1 = 0x02;
const HTControllerLoc HTDaisyChainPosition2 = 0x04;
const HTControllerLoc HTDaisyChainPosition3 = 0x06;
const HTControllerLoc HTDaisyChainPosition4 = 0x08;

// signatures of the servo and motor controllers
string _HTServoSignature = "HiTechncServoCon";
string _HTMotorSignature = "HiTechncMotorCon";

/*
 * Needs to be documented
 */
void verifyHTController(tSensors link, HTControllerType controllerType, HTControllerLoc location) {

   char I2CRequest[3];
   char I2CResponse[17];
   string _responseMsg;

   // verify that the Sensor is configured to the correct type
   if (SensorType[link]!=sensorI2CMuxController) goto FailBadSensorType;

   // wait for any pending comm ... shouldn't have any but just in case
   while(nI2CStatus[link]==STAT_COMM_PENDING) Sleep(1);

   // change the sensor type to custom ... remember the configured sensor type so we can change it back
  TSensorTypes origSensorType = SensorType[link];
  SensorType[link]=sensorI2CCustom;

  // construct the message to get the type of controller
   // size of message
   I2CRequest[0]=2;
   // address on the I2C Bus (0x02 is the first motor controller / first motor
   I2CRequest[1]=location;
   // starting address of what we want back
   I2CRequest[2]=0x08;

   // send the request, expecting a 16 bit response
   sendI2CMsg(link,I2CRequest,16);

   // wait while we have the I2C message bus active
   while(nI2CStatus[link]==STAT_COMM_PENDING) Sleep(1);

   // initial check ... did we get the right status and the right number of bytes?
   int _bytesReady=nI2CBytesReady[link];
   TI2CStatus _status = nI2CStatus[link];
   if (_status!=NO_ERR || _bytesReady!=16) {
      SensorType[link]=origSensorType;
      goto FailBadController;
   }

   // if so, read the reply
   readI2CReply(link,I2CResponse,16);

   // we're now done with critical processing ... return the sensorType back the original value
   SensorType[link]=origSensorType;

   // convert the reponse message to a string
   I2CResponse[16]=(char)0;
   StringFromChars(_responseMsg,I2CResponse);

   // check the message for correctness
   if (controllerType==HTServoController && strcmp(_responseMsg,_HTServoSignature)!=0) goto FailBadController;
   if (controllerType==HTMotorController && strcmp(_responseMsg,_HTMotorSignature)!=0) goto FailBadController;

   // Look like we have a good connection ... we can return
   return;


   //******************************************************************
   // Failure condition ... invalid controller (or no controller) found
   //******************************************************************
FailBadController:
   // Display the expected controller type, port, and location
   eraseDisplay();
  nxtDisplayCenteredTextLine(0,"CONTROLLER ERROR");
  nxtDisplayTextLine(2,"%s controller",controllerType);
  nxtDisplayTextLine(3,"expected.");
  switch(link)
  {
     case S1:
        nxtDisplayTextLine(5,"  Port: S1");
        break;
     case S2:
        nxtDisplayTextLine(5,"  Port: S2");
        break;
     case S3:
        nxtDisplayTextLine(5,"  Port: S3");
        break;
     case S4:
        nxtDisplayTextLine(5,"  Port: S4");
        break;
     default:
        nxtDisplayTextLine(5,"  Port: unknown");
        break;
  }
  nxtDisplayTextLine(6,"  Location: %i",(int)location/2);

   // wait forever...
   while(true){

      PlayImmediateTone(1000, 30);
      PlayImmediateTone(700, 30);

   }

   //*************************************************************
   // Failure condition ... sensor port is not of the correct type
   //*************************************************************
FailBadSensorType:
   // Display the Sensor Port and Type
   eraseDisplay();
  nxtDisplayCenteredTextLine(0,"SENSORPORT ERROR");
  nxtDisplayTextLine(2,"SensorType = %i",SensorType[link]);
  nxtDisplayTextLine(3,"  (Expected 26)");
  switch(link)
  {
     case S1:
        nxtDisplayTextLine(5,"  Port: S1");
        break;
     case S2:
        nxtDisplayTextLine(5,"  Port: S2");
        break;
     case S3:
        nxtDisplayTextLine(5,"  Port: S3");
        break;
     case S4:
        nxtDisplayTextLine(5,"  Port: S4");
        break;
     default:
        nxtDisplayTextLine(5,"  Port: unknown");
        break;
  }

  // wait forever...
   while(true){

      PlayImmediateTone(1000, 30);
      PlayImmediateTone(700, 30);

   }

}



Sat Dec 06, 2014 10:21 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.