Combination Report Einstein Robot



Download 10.84 Mb.
Page14/41
Date conversion08.07.2018
Size10.84 Mb.
1   ...   10   11   12   13   14   15   16   17   ...   41

6 To-Do

6.1 Base "To-Do" List

NXT++

Because RobotC might be limited with image processing and sensor integration, we may need to migrate our platform to NXT++. In addition, in order to accommodate more sensors than ports that we have, it might be wise to migrate over. Another reason we are thinking about switching over is because of the experience some of the members have had with image processing. All in all, depending on the research that we do into the ability of RobotC, we may stick with it.


Camera Sensors


We want to implement a stereoscopic camera system. There are significant technical considerations that are involved in order to determine distances and sizes using stereo image processing. We hope to be able to use these cameras for image recognition and mapping.

Sonar Sensors


We would like to use sonar sensors to create another map grid. These will have a very similar purpose to the cameras, but will mainly be used for obstacle mapping.

Environment Mapping


Utilize the grids generated by our sensors, and explore until an object of interest is sighted.

Image processing


Interpret data gathered and look for objects of interest. There will be many avenues to take, and we will need to research this throughout the semester before implementing it.

Genetic Algorithms


Once we have located an object of interest, find the most efficient path and implement it.

Infrared Sensor/Laser Sensor


Infrared sensors provide a narrower area for distance detection which is advantageous for detection of distances for a specific object or target. However, many scenarios exist where interference can cause poor readings or surfaces that may not reflect in the IR range. A laser based measurement sensor would work very well in this scenario but is much more expensive to implement. We can union this with our other map grids generated with other sensors.
6.2 Head "To-Do" List
For the continued development of the head, there are several areas that we are considering looking into. We would like to eventually have the head to have more of a personality and greater range of emotion then it currently has. We feel this can be done through the use of genetic algorithms and/or genetic programming – though it is possible that evolutionary programming might be the best way to allow the robot to develop its own emotions.
We would like to save snippets of motion into separate files that can then be called within a lisp tree. Then by adding a camera for vision to allow the robot to recognize facial expression, the robot can then learn which types of facial expressions would work best. This might be done through mimicry for some of the basic emotions.
A large population of emotions would be needed to compete in some way, then a fitness function would then have to be developed to find those that perform best allowing those to continue, while removing the lowest performers. The most difficult part of this process would be defining the fitness function. In fact, the fitness function would probably derive its feedback from human input as the robot would have no way of knowing whether it is producing and acceptable expression or not. The fitness function could be as simple as a human observing the results of a genetic modification and giving it a 'thumbs-up' or a 'thumbs-down' for feedback into the system. The feedback might also be multi-valued such that an expression could be rated 'good' 'bad' or 'invalid expression'. It could also be rated (say, 1 to 5) for relative degree of success.
Whatever the fitness function properties, the fact that it will probably have to be derived from human input will severely limit the amount of generations that can be expressed in a reasonable amount of time. The best use of time may be to have basic behaviors mimicked (as described above) and then have some of the parameters genetically evolve with restrictions to only very slight changes. This could give rise to a nice library of variations of behavior such as "happy smile", "sad smile", "grimace", "gleeful smile", "snickering smile" and similar variations. The variations would have to be categorized by humans and a tree of related emotions built to where there may be several inputs available and the robot may be able to choose the "happy" category, followed by "grimace" if, say, a sensor indicated that it bumped into an unexpected object.
This technique is currently being done on a “simpler” style of robot, than the robot we are working on. Those that perform worst are removed from the population, and replaced by a new set, which have new behaviors based on those of the winners. Over time the population improves, and eventually a satisfactory robot may appear. This can happen without any direct programming of the robots by the researchers. This method is being used to both create better robots and to explore the nature of evolution. However, since the process often requires many generations of robots to be simulated, this technique may be run entirely or mostly in simulation, then tested on real robots once the evolved algorithms are good enough. (Patch, 2004)
For behaviors, most of the algorithm probably couldn’t be simulated with the tools that we currently have on hand, so instead the robot would have to have feedback from us for what emotions were acceptable and which are not.
6.3 Arm "To-Do" List
Sensors

Future work that can be performed on this mechanical arm includes adding sensors. These could include sonar range finder, stereo (two) camera vision, or even an experimental smell detector. This would allow automation of the entire robot

Specify End-effecter Position
Additionally, and more specifically for the mechanical arm, future work could involve solving the inverse kinematic equations for all degrees of freedom in the arm. This would allow the user or an automated intelligent program utilizing sensors to specify a position and orientation that the hand should be in. All the angles of rotation for each motor and servo would be automatically calculated and moved to that position.
Trajectory planning

With the addition of sensors, the arm and hand could potentially utilize trajectory planning. This would entail sensing an object coming toward the robot, calculating its speed and trajectory, and moving the arm and hand to a position along that trajectory to potentially deflect or catch the incoming object. The movement of the arm and hand would have to be sped up and position control accuracy would have to be increased for this to be possible.

Genetic Search Algorithm

As long as memory in the NXT brick allows, genetic algorithms could be implemented to allow for room mapping and searching. This would allow the robot, and more specifically the arm, to know the position of objects in the room and potentially interact with them.


Image processing

Image processing would be an essential upgrade with vision sensors so that the incoming data could be interpreted properly. Intelligent processing would allow more accurate readings and would provide optimized responses.


Head Appendix

Appendix A – Servo Table



Purpose of servo

Servo manufacturer and model number

Location on Frame

Device number on controller

Maximum Position

Minimum Position

Default Position

Rotate head

Hitec HS-700BB

Base of neck

12

254

0

127

Tilt base of neck

Hitec HS-805BB+



4

254

0

127

Nod head forward

Hitec HS-805BB+



0

254

58

156

Tilt head left & right

Hitec HS-705MG

Top of neck

9

254

0

125

Open mouth

Futaba S3003

Left “ear” location

11

254

0

0




Futaba S3003

Right “ear” location (not used)













Right Eyebrow up

Hitec HS-300

Left-top of head

10

254

0

250

Left Eyebrow up

Hitec HS-300

Right-top of head

7

254

0

0

Mouth left-corner

Hitec HS-475HB




14

175

50

127

Mouth right-corner

Airtronics 94102




1

220

85

127

Eyes up/down

Futaba S3003




15

254

0

139

Eyes left/right

Hitec HS-55

Center of eyes

2

254

0

127


Appendix B - Files used in animation


Note that all the following files were placed in the C:\VSA directory

  • happy.wav - sound file for happy, a laugh.

  • happy.vsa - the motion file for neutral - happy -> sad as one continuous motion

  • stay_happy.vsa - the motion file for a neutral -> happy transition. Does not reuturn the head to neutral.

  • shrthppy.bat - calls VSA with happy.vsa to produce the neutral->happy-> sad motion sequence

    • contents of file:

      • vsa "happy.vsa" /play /minimize /close

  • longghappy.bat - calls VSA with stay_happy.vsa to produce the neutral-> happy state.

    • Contents of file:

      • vsa "stay_happy.vsa" /play /minimize /close




  • neutral.vsa - motion sequence file to reset all servos to the neutral position. Tends to be jerky since there is no velocity control for return to neutral when the present state is unknown ahead of time.

  • neutral.bat - calls VSA with neutral.vsa to reutrn the head to neutral. Used after longhappy.bat and longsad.bat

    • contents of file:

      • vsa "neutral.vsa" /play /minimize /close




  • sad.wav - sound file for sad emotions, a groan

  • sad.vsa - the motion file for neutral - sad - neutral as one continuous motion

  • stay_sad.vsa - the motion file for neutral -> sad transition. Does not return to neutral

  • shortsad.bat - calls VSA with sad.vsa to produce the neutral-sad-neutral motion sequence

    • contents of file:

      • vsa "sad.vsa" /play /minimize /close

  • longsad.bat - calls the VSA with stay_sad to produce the neutral-> sad state.

    • Contents of file:

      • vsa "stay_sad.vsa" /play /minimize /close

8 Base Appendix

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

/* */


/* Program Name: PSP-Nx-lib.c */

/* =========================== */

/* */

/* Copyright (c) 2008 by mindsensors.com */



/* Email: info () mindsensors () com */

/* */


/* This program is free software. You can redistribute it */

/* and/or modify it under the terms of the GNU General */

/* Public License as published by the Free Software */

/* Foundation; version 3 of the License. */

/* Read the license at: http://www.gnu.org/licenses/gpl.txt */

/* */


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

* History

* ------------------------------------------------

* Author Date Comments

* Deepak 04/08/09 Initial Authoring.

*/

/*--------------------------------------



Controller button layout:

----------------------------------------


L1 R1

L2 R2
d triang

a c square circle

b cross
l_j_b r_j_b

l_j_x r_j_x

l_j_y r_j_y


-------------------------------------- */

/*

bits as follows:



b1: a b c d x r_j_b l_j_b x

b2: square cross circle triang R1 L1 R2 L2

*/
typedef struct {

char b1; //raw byte read from PSP-Nx

char b2; //raw byte read from PSP-Nx
// computed button states

char l1;


char l2;

char r1;


char r2;

char a;


char b;

char c;


char d;

char triang;

char square;

char circle;

char cross;

char l_j_b; // joystick button state

char r_j_b; // joystick button state


Appendix A: Entire Program Code


int l_j_x; // analog value of joystick scaled from 0 to 100

int l_j_y; // analog value of joystick scaled from 0 to 100

int r_j_x; // analog value of joystick scaled from 0 to 100

int r_j_y; // analog value of joystick scaled from 0 to 100

} psp;
void PSP_SendCommand(tSensors port, byte i2cAddr, byte command)

{

byte msg[5];


// Build the I2C message

msg[0] = 3;

msg[1] = i2cAddr;

msg[2] = 0x41;

msg[3] = command;
// Wait for I2C bus to be ready

while (nI2CStatus[port] == STAT_COMM_PENDING);

// when the I2C bus is ready, send the message you built

sendI2CMsg(port, msg[0], 0);

while (nI2CStatus[port] == STAT_COMM_PENDING);

}
void PSP_ReadButtonState(tSensors port, byte i2cAddr, psp & currState)

{

byte msg[5];



unsigned byte replyMsg[7];

byte b0, b1;


msg[0] = 2;

msg[1] = i2cAddr;

msg[2] = 0x42;
currState.b1 = 0;

currState.b2 = 0;

currState.l1 = 0;

currState.l2 = 0;

currState.r1 = 0;

currState.r2 = 0;

currState.a = 0;

currState.b = 0;

currState.c = 0;

currState.d = 0;

currState.triang = 0;

currState.square = 0;

currState.circle = 0;

currState.cross = 0;

currState.l_j_b = 0;

currState.r_j_b = 0;

currState.l_j_x = 0;

currState.l_j_y = 0;

currState.r_j_x = 0;

currState.r_j_y = 0;

while (nI2CStatus[port] == STAT_COMM_PENDING)

{

// Wait for I2C bus to be ready



}

// when the I2C bus is ready, send the message you built

sendI2CMsg (port, msg[0], 6);

while (nI2CStatus[port] == STAT_COMM_PENDING)

{

// Wait for I2C bus to be ready



}



// read back the response from I2C

readI2CReply (port, replyMsg[0], 6);

b0 = replyMsg[0]&0xff;

b1 = replyMsg[1]&0xff;


currState.b1 = b0;

currState.b2 = b1;


currState.l_j_b = (b0 >> 1) & 0x01;

currState.r_j_b = (b0 >> 2) & 0x01;


currState.d = (b0 >> 4) & 0x01;

currState.c = (b0 >> 5) & 0x01;

currState.b = (b0 >> 6) & 0x01;

currState.a = (b0 >> 7) & 0x01;


currState.l2 = (b1 ) & 0x01;

currState.r2 = (b1 >> 1) & 0x01;

currState.l1 = (b1 >> 2) & 0x01;

currState.r1 = (b1 >> 3) & 0x01;

currState.triang = (b1 >> 4) & 0x01;

currState.circle = (b1 >> 5) & 0x01;

currState.cross = (b1 >> 6) & 0x01;

currState.square = (b1 >> 7) & 0x01;


currState.l_j_x = (((replyMsg[2]&0xff) - 128) * 100)/128;

currState.l_j_y = (((replyMsg[3]&0xff) - 128) * 100)/128;

currState.r_j_x = (((replyMsg[4]&0xff) - 128) * 100)/128;

currState.r_j_y = (((replyMsg[5]&0xff) - 128) * 100)/128;


}


NXT HID library code



/*

* $Id: MSHID-driver.h 16 2009-09-24 18:55:30Z xander $

*/
/* \file MSHID-driver.h

* \brief Mindsensors HID Sensor driver

*

* MSHID-driver.h provides an API for the Mindsensors HID



* Sensor.

*

* Changelog:



* - 0.1: Initial release

*

* Credits:



* - Big thanks to Mindsensors for providing me with the

* hardware necessary to write and test this.

*

* License: You may use this code as you wish, provided you



* give credit where its due.

*

* THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 1.46 AND



* HIGHER.

* \author Xander Soldaat (mightor_at_gmail.com)

* \date 19 July 2009

* \version 0.1

* \example MSHID-test1.c

*/
#ifndef __MSHID_H__

#define __MSHID_H__
#ifndef __COMMON_H__

#include "common.h"

#endif
#define MSHID_I2C_ADDR 0x04

#define MSHID_CMD 0x41

#define MSHID_KEYBMOD 0x42

#define MSHID_KEYBDATA 0x43


#define MSHID_XMIT 0x54

#define MSHID_ASCII 0x41

#define MSHID_DDATA 0x44
// Keyboard modifyers

#define MSHID_MOD_NONE 0x00

#define MSHID_MOD_LCTRL 0x01

#define MSHID_MOD_LSHIFT 0x02

#define MSHID_MOD_LALT 0x04

#define MSHID_MOD_LGUI 0x08

#define MSHID_MOD_RCTRL 0x10

#define MSHID_MOD_RSHIFT 0x20

#define MSHID_MOD_RALT 0x40

#define MSHID_MOD_RGUI 0x80


/*!< Array to hold I2C command data */

tByteArray MSHID_I2CRequest;







/*

* Send a direct command to the HID sensor

* @param link the HID port number

* @param command the command to be sent

* @return true if no error occured, false if it did

*/

bool MSHIDsendCommand(tSensors link, byte command) {



memset(MSHID_I2CRequest, 0, sizeof(tByteArray));

MSHID_I2CRequest.arr[0] = 3;

MSHID_I2CRequest.arr[1] = MSHID_I2C_ADDR;

MSHID_I2CRequest.arr[2] = MSHID_CMD;

MSHID_I2CRequest.arr[3] = command;
return writeI2C(link, MSHID_I2CRequest, 0);

}

/**



* Send keyboard data to the HID sensor. Must be followed by a

* MSHID_XMIT

* command using MSHIDsendCommand()

* @param link the HID port number

* @param modifier the keyboard modifier, like shift, control.

* Can be OR'd together.

* @param keybdata the keystroke to be sent to the computer

* @return true if no error occured, false if it did

*/

bool MSHIDsendKeyboardData(tSensors link, byte modifier, byte keybdata) {



memset(MSHID_I2CRequest, 0, sizeof(tByteArray));

MSHID_I2CRequest.arr[0] = 4;

MSHID_I2CRequest.arr[1] = MSHID_I2C_ADDR;

MSHID_I2CRequest.arr[2] = MSHID_KEYBMOD;

MSHID_I2CRequest.arr[3] = modifier;

MSHID_I2CRequest.arr[4] = keybdata;


return writeI2C(link, MSHID_I2CRequest, 0);

}

/**



* Send a string to the computer. Can be up to 19 characters long.

* It recognises the following escaped keys:


* - \n: new line

* - \r: carriage return

* - \t: tab

* - \\: a backslash

* - \": double quote

* @param link the HID port number

* @param data the string to be transmitted

* @return true if no error occured, false if it did

*/

bool MSHIDsendString(tSensors link, string data) {



byte buffer[19];

int len = strlen(data);

if (len < 20) {

memcpy(buffer, data, len);

} else {

return false;

}



for (int i = 0; i < len; i++) {

if (buffer[i] == 0x5C && i < (len - 1)) {

switch (buffer[i+1]) {

case 'r':

if (!MSHIDsendKeyboardData(link, MSHID_MOD_NONE, 0x0A))

return false;

break;


case 'n':

if (!MSHIDsendKeyboardData(link, MSHID_MOD_NONE, 0x0D))

return false;

break;


case 't':

if (!MSHIDsendKeyboardData(link, MSHID_MOD_NONE, 0x09))

return false;

break;


case 0x5C:

if (!MSHIDsendKeyboardData(link, MSHID_MOD_NONE, 0x5C))

return false;

break;


case 0x22:

if (!MSHIDsendKeyboardData(link, MSHID_MOD_NONE, 0x22))

return false;

break;


default:

break;


}

i++;


} else {

if (!MSHIDsendKeyboardData(link, MSHID_MOD_NONE, buffer[i]))

return false;

}

if (!MSHIDsendCommand(link, MSHID_XMIT))



return false;

wait1Msec(50);

}

return true;



}
#endif // __MSHID_H__
/*

* $Id: MSHID-driver.h 16 2009-09-24 18:55:30Z xander $

*/



common.h


This is a commonly used library for various drivers.


/*

* $Id: common.h 17 2009-10-30 11:24:51Z xander $

*/
/** \file common.h

* \brief Commonly used functions used by drivers.

*

* common.h provides a number of frequently used functions that



* are useful for writing drivers.

* License: You may use this code as you wish, provided you give

* credit where its due.

* THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 1.46 AND HIGHER.

*

* Changelog:



* - 0.1: Initial release

* - 0.2: Added version check to issue error when compiling with

* RobotC < 1.46

* - 0.2: Added __COMMON_H_DEBUG__ to enable/disable sounds when

* an I2C error occurs

* - 0.2: Removed bool waitForI2CBus(tSensors link, bool silent)

* - 0.3: clearI2CError() added to make writeI2C more robust,

* I2C bus errors are now handled better.

* - 0.4: Added HiTechnic SMUX functions

* - 0.5: Added clip function (Tom Roach)

* - 0.6: clearI2CBus is now conditionally compiled into the FW.

* Only RobotC < 1.57 needs it.

* - 0.7: ubyteToInt(byte byteVal) modified, works better with

* 1.57+


* - 0.8: ubyte used for arrays for firmware version 770 and

* higher

* added support for new colour sensor

* added better handling for when sensor is not

* configured properly

*

* \author Xander Soldaat (mightor_at_gmail.com)



* \date 24 September 2009

* \version 0.8

*/
#ifndef __COMMON_H__

#define __COMMON_H__


#undef __COMMON_H_DEBUG__

//#define __COMMON_H_DEBUG__

#include "firmwareVersion.h"

#if (kFirmwareVersion < 760)

#error "These drivers are only supported on RobotC version 1.46 or higher"

#endif


/*!< Convert tMUXSensor to sensor port number */

#define SPORT(X) X / 4

/*!< Convert tMUXSensor to MUX port number */

#define MPORT(X) X % 4


#ifndef MAX_ARR_SIZE

/**


* Maximum buffer size for byte_array, can be overridden in your

* own program.

* It's 17 bytes big because the max I2C buffer size is 16, plus

* 1 byte to denote packet length.

*/

#define MAX_ARR_SIZE 17



#endif




/*!< HTSMUX I2C device address */

#define HTSMUX_I2C_ADDR 0x10

/*!< Command register */

#define HTSMUX_COMMAND 0x20

/*!< Status register */

#define HTSMUX_STATUS 0x21


// Registers

/*!< Sensor mode register */

#define HTSMUX_MODE 0x00

/*!< Sensor type register */

#define HTSMUX_TYPE 0x01

/*!< I2C byte count register */

#define HTSMUX_I2C_COUNT 0x02

/*!< I2C device address register */

#define HTSMUX_I2C_DADDR 0x03

/*!< I2C memory address register */

#define HTSMUX_I2C_MADDR 0x04

/*!< Channel register offset */

#define HTSMUX_CH_OFFSET 0x22

/*!< Number of registers per sensor channel */

#define HTSMUX_CH_ENTRY_SIZE 0x05

/*!< Analogue upper 8 bits register */

#define HTSMUX_ANALOG 0x36

/*!< Number of registers per analogue channel */

#define HTSMUX_AN_ENTRY_SIZE 0x02
/*!< I2C buffer register offset */

#define HTSMUX_I2C_BUF 0x40

/*!< Number of registers per buffer */

#define HTSMUX_BF_ENTRY_SIZE 0x10


// Command fields

/*!< Halt multiplexer command */

#define HTSMUX_CMD_HALT 0x00

/*!< Start auto-detect function command */

#define HTSMUX_CMD_AUTODETECT 0x01

/*!< Start normal multiplexer operation command */

#define HTSMUX_CMD_RUN 0x02
// Status

/*!< Nothing going on, everything's fine */

#define HTSMUX_STAT_NORMAL 0x00

/*!< No battery voltage detected status */

#define HTSMUX_STAT_BATT 0x01

/*!< Auto-dected in progress status */

#define HTSMUX_STAT_BUSY 0x02

/*!< Multiplexer is halted status */

#define HTSMUX_STAT_HALT 0x04

/*!< Command error detected status */

#define HTSMUX_STAT_ERROR 0x08

/*!< Status hasn't really been set yet */

#define HTSMUX_STAT_NOTHING 0xFF

// Channel modes

/*!< I2C channel present channel mode */

#define HTSMUX_CHAN_I2C 0x01

/*!< Enable 9v supply on analogue pin channel mode */

#define HTSMUX_CHAN_9V 0x02

/*!< Drive pin 0 high channel mode */

#define HTSMUX_CHAN_DIG0_HIGH 0x04

/*!< Drive pin 1 high channel mode */

#define HTSMUX_CHAN_DIG1_HIGH 0x08

/*!< Set slow I2C rate channel mode */

#define HTSMUX_CHAN_I2C_SLOW 0x10






/**


* Array of bytes as a struct, this is a work around for RobotC's

* inability to pass an array to

* a function. The int has to be there or it won't work.

*/
typedef struct {

#if (kFirmwareVersion < 770)

byte arr[MAX_ARR_SIZE];

int placeholder;

#else


ubyte arr[MAX_ARR_SIZE];

#endif


} tByteArray;

typedef struct {

#if (kFirmwareVersion < 770)

byte arr[MAX_ARR_SIZE];

int placeholder;

#else


sbyte arr[MAX_ARR_SIZE];

#endif


} tsByteArray;
/**

* Array of ints as a struct, this is a work around for RobotC's

* inability to pass an array to

* a function. The byte has to be there or it won't work.

*/

typedef struct {



int arr[MAX_ARR_SIZE];

#if (kFirmwareVersion < 770)

byte placeholder;

#endif


} tIntArray;
/*!< Sensor types as detected by SMUX */

typedef enum

{

HTSMUXAnalogue = 0x00,



HTSMUXLegoUS = 0x01,

HTSMUXCompass = 0x02,

HTSMUXColor = 0x03,

HTSMUXAccel = 0x04,

HTSMUXIRSeeker = 0x05,

HTSMUXProto = 0x06,

HTSMUXColorNew = 0x07,

HTSMUXIRSeekerNew = 0x09,

HTSMUXSensorNone = 0xFF

} HTSMUXSensorType;






/*!< Sensor and SMUX port combinations */

typedef enum {

msensor_S1_1 = 0,

msensor_S1_2 = 1,

msensor_S1_3 = 2,

msensor_S1_4 = 3,

msensor_S2_1 = 4,

msensor_S2_2 = 5,

msensor_S2_3 = 6,

msensor_S2_4 = 7,

msensor_S3_1 = 8,

msensor_S3_2 = 9,

msensor_S3_3 = 10,

msensor_S3_4 = 11,

msensor_S4_1 = 12,

msensor_S4_2 = 13,

msensor_S4_3 = 14,

msensor_S4_4 = 15

} tMUXSensor;


/*!< Struct to hold SMUX info */

typedef struct {

bool initialised;

/*!< Has the MMUX been initialised yet? */

byte status;

/*!< SMUX status */

HTSMUXSensorType sensor[4];

/*!< What kind of sensor is attached to this port */

} smuxDataT;
smuxDataT smuxData[4];

/*!< Holds all the MMUX info, one for each sensor port */

tByteArray HTSMUX_I2CRequest;

/*!< Array to hold I2C command data */

tByteArray HTSMUX_I2CReply;

/*!< Array to hold I2C reply data */


#if (kFirmwareVersion < 770)

void clearI2CBus(tSensors link);

#endif
void clearI2CError(tSensors link, byte address);

bool waitForI2CBus(tSensors link);

bool writeI2C(tSensors link, tByteArray &data, int replylen);

bool readI2C(tSensors link, tByteArray &data, int replylen);

byte HTSMUXreadStatus(tSensors link);

HTSMUXSensorType HTSMUXreadSensorType(tSensors link, byte channel);

bool HTSMUXscanPorts(tSensors link);

bool HTSMUXsendCommand(tSensors link, byte command);

bool HTSMUXreadPort(tSensors link, byte channel, tByteArray &result, int numbytes);

int HTSMUXreadAnalogue(tSensors link, byte channel);

int min(int x1, int x2);

int max(int x1, int x2);

int ubyteToInt(byte byteVal);
/**

* Clear out the stale bytes in the I2C bus before we send new

* data

* Note: As of RobotC 1.57 (770) this function has become



* obsolete.

* @param link the port number

*/





#if (kFirmwareVersion < 770)

void clearI2CBus(tSensors link) {

ubyte _tmp = 0;

while (nI2CBytesReady[link] > 0)

readI2CReply(link, _tmp, 1);

}

#endif


/**

* Clear out the error state on I2C bus by sending a bunch of

* dummy packets.

* @param link the port number

* @param address the I2C address we're sending to

*/

void clearI2CError(tSensors link, byte address) {



byte error_array[2];

error_array[0] = 1; // Message size

error_array[1] = address; // I2C Address

#ifdef __COMMON_H_DEBUG__

eraseDisplay();

nxtDisplayTextLine(3, "rxmit: %d", ubyteToInt(error_array[1]));

wait1Msec(2000);

#endif // __COMMON_H_DEBUG__


for (int i = 0; i < 5; i++) {

sendI2CMsg(link, error_array[0], 0);

wait1Msec(5);

}

}



/**

* Wait for the I2C bus to be ready for the next message

* @param link the port number

* @return true if no error occured, false if it did

*/

bool waitForI2CBus(tSensors link)



{

//TI2CStatus i2cstatus;

while (true)

{

//i2cstatus = nI2CStatus[link];



switch (nI2CStatus[link])

//switch(i2cstatus)

{

case NO_ERR:



return true;
case STAT_COMM_PENDING:

break;
case ERR_COMM_CHAN_NOT_READY:

break;
case ERR_COMM_BUS_ERR:

#ifdef __COMMON_H_DEBUG__

PlaySound(soundLowBuzz);

while (bSoundActive) {}

#endif // __COMMON_H_DEBUG__

#if (kFirmwareVersion < 770)

clearI2CBus(link);

#endif


return false;

}

}



}




/**

* Write to the I2C bus. This function will clear the bus and

* wait for it be ready before any bytes are sent.

* @param link the port number

* @param data the data to be sent

* @param replylen the number of bytes (if any) expected in reply to this command

* @return true if no error occured, false if it did

*/

bool writeI2C(tSensors link, tByteArray &data, int replylen) {



#if (kFirmwareVersion < 770)

clearI2CBus(link);

#endif

if (!waitForI2CBus(link)) {



clearI2CError(link, data.arr[1]);
// Let's try the bus again, see if the above packets flushed it out

// clearI2CBus(link);

if (!waitForI2CBus(link))

return false;

}
sendI2CMsg(link, data.arr[0], replylen);
if (!waitForI2CBus(link)) {

clearI2CError(link, data.arr[1]);

sendI2CMsg(link, data.arr[0], replylen);

if (!waitForI2CBus(link))

return false;

}

return true;



}

/**


* Read from the I2C bus. This function will wait for the bus

* to be ready before reading from it.

* @param link the port number

* @param data holds the data from the reply

* @param replylen the number of bytes in the reply

* @return true if no error occured, false if it did

*/

bool readI2C(tSensors link, tByteArray &data, int replylen) {



// clear the input data buffer

memset(data, 0, sizeof(tByteArray));

// wait for the bus to be done receiving data

if (!waitForI2CBus(link))

return false;

// ask for the input to put into the data array

readI2CReply(link, data.arr[0], replylen);

return true;

}
/*

* Initialise the smuxData array needed for keeping track of

* sensor settings

*/

void HTSMUXinit(){



for (int i = 0; i < 4; i++) {

memset(smuxData[i].sensor, 0xFF, sizeof(HTSMUXSensorType)*4);

smuxData[i].status = HTSMUX_STAT_NOTHING;

smuxData[i].initialised = true;

}

}





/**

* Read the status of the SMUX

*

* The status byte is made up of the following bits:



*

* | D7 | D6 | D4 | D3 | D2 | D1 | D1 |

* -D1 - HTSMUX_STAT_BATT: No battery voltage detected

* -D2 - HTSMUX_STAT_BUSY: Auto-dected in progress status

* -D3 - HTSMUX_STAT_HALT: Multiplexer is halted

* -D4 - HTSMUX_STAT_ERROR: Command error detected

* @param link the SMUX port number

* @return the status byte

*/

byte HTSMUXreadStatus(tSensors link) {



memset(HTSMUX_I2CRequest, 0, sizeof(tByteArray));
HTSMUX_I2CRequest.arr[0] = 2; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_STATUS;
if (!writeI2C(link, HTSMUX_I2CRequest, 1))

return -1;


if (!readI2C(link, HTSMUX_I2CReply, 1))

return -1;


return HTSMUX_I2CReply.arr[0];

}

/**



* Get the sensor type attached to specified SMUX port

*

* The status byte is made up of the following bits:



*

* | D7 | D6 | D4 | D3 | D2 | D1 | D1 |

* -D1 - HTSMUX_STAT_BATT: No battery voltage detected

* -D2 - HTSMUX_STAT_BUSY: Auto-dected in progress status

* -D3 - HTSMUX_STAT_HALT: Multiplexer is halted

* -D4 - HTSMUX_STAT_ERROR: Command error detected

* @param link the SMUX port number

* @param channel the SMUX channel number

* @return the status byte

*/

HTSMUXSensorType HTSMUXreadSensorType(tSensors link, byte channel) {



return smuxData[link].sensor[channel];

}

/**



* Set the mode of a SMUX channel.

*

* Mode can be one or more of the following:



* -HTSMUX_CHAN_I2C

* -HTSMUX_CHAN_9V

* -HTSMUX_CHAN_DIG0_HIGH

* -HTSMUX_CHAN_DIG1_HIGH

* -HTSMUX_CHAN_I2C_SLOW

* @param link the SMUX port number

* @param channel the SMUX channel number

* @param mode the mode to set the channel to

* @return true if no error occured, false if it did

*/




bool HTSMUXsetMode(tSensors link, byte channel, byte mode) {

// If we're in the middle of a scan, abort this call

if (smuxData[link].status == HTSMUX_STAT_BUSY) {

return false;

} else if (smuxData[link].status != HTSMUX_STAT_HALT) {

// Always make sure the SMUX is in the halted state

if (!HTSMUXsendCommand(link, HTSMUX_CMD_HALT))

return false;

wait1Msec(50);

}
memset(HTSMUX_I2CRequest, 0, sizeof(tByteArray));
HTSMUX_I2CRequest.arr[0] = 3; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_CH_OFFSET + HTSMUX_MODE + (HTSMUX_CH_ENTRY_SIZE * channel);

HTSMUX_I2CRequest.arr[3] = mode;


if (!writeI2C(link, HTSMUX_I2CRequest, 0))

return false;


if (!readI2C(link, HTSMUX_I2CReply, 0))

return false;


return true;

}

/**



* Scan the specified SMUX's channels and configure them.

*

* Note: this functions takes 500ms to return while the scan is



* in progress.

* @param link the SMUX port number

* @return true if no error occured, false if it did

*/

bool HTSMUXscanPorts(tSensors link) {



// If we're in the middle of a scan, abort this call

if (smuxData[link].status == HTSMUX_STAT_BUSY) {

return false;

}
// Always make sure the SMUX is in the halted state

if (!HTSMUXsendCommand(link, HTSMUX_CMD_HALT))

return false;

wait1Msec(100);
// Commence scanning the ports and allow up to 500ms to complete

if (!HTSMUXsendCommand(link, HTSMUX_CMD_AUTODETECT))

return false;

wait1Msec(500);

smuxData[link].status = HTSMUX_STAT_HALT;
for (int i = 0; i < 4; i++) {

memset(HTSMUX_I2CRequest, 0, sizeof(tByteArray));


HTSMUX_I2CRequest.arr[0] = 2; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_CH_OFFSET + HTSMUX_TYPE + (HTSMUX_CH_ENTRY_SIZE * i);




if (!writeI2C(link, HTSMUX_I2CRequest, 1))

smuxData[link].sensor[i] = HTSMUXSensorNone;
if (!readI2C(link, HTSMUX_I2CReply, 1))

smuxData[link].sensor[i] = HTSMUXSensorNone;


smuxData[link].sensor[i] = (tMUXSensor)ubyteToInt(HTSMUX_I2CReply.arr[0]);

}
// Work-around for galloping buffer problem, applies to the HTPBs only.

for (int i = 0; i < 4; i++) {

if (smuxData[link].sensor[i] == HTSMUXProto) {

HTSMUX_I2CRequest.arr[0] = 3; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_CH_OFFSET + HTSMUX_I2C_COUNT + (HTSMUX_CH_ENTRY_SIZE * i);

HTSMUX_I2CRequest.arr[3] = 14;

if (!writeI2C(link, HTSMUX_I2CRequest, 0))

smuxData[link].sensor[i] = HTSMUXSensorNone;

}

}

return true;



}

/**


* Send a command to the SMUX.

* command can be one of the following:

* -HTSMUX_CMD_HALT

* -HTSMUX_CMD_AUTODETECT

* -HTSMUX_CMD_RUN

*

* in progress.



* @param link the SMUX port number

* @param command the command to be sent to the SMUX

* @return true if no error occured, false if it did

*/

bool HTSMUXsendCommand(tSensors link, byte command) {



memset(HTSMUX_I2CRequest, 0, sizeof(tByteArray));
HTSMUX_I2CRequest.arr[0] = 3; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_COMMAND;

HTSMUX_I2CRequest.arr[3] = command;


switch(command) {

case HTSMUX_CMD_HALT:

smuxData[link].status = HTSMUX_STAT_HALT;

break;


case HTSMUX_CMD_AUTODETECT:

smuxData[link].status = HTSMUX_STAT_BUSY;

break;

case HTSMUX_CMD_RUN:



smuxData[link].status = HTSMUX_STAT_NORMAL;

break;


}

return writeI2C(link, HTSMUX_I2CRequest, 0);

}




/**

* Read the value returned by the sensor attached the SMUX. This function

* is for I2C sensors.

*

* @param link the SMUX port number



* @param channel the SMUX channel number

* @param result array to hold values returned from SMUX

* @param numbytes the size of the I2C reply

* @param offset the offset used to start reading from

* @return true if no error occured, false if it did

*/

bool HTSMUXreadPort(tSensors link, byte channel, tByteArray &result, int numbytes, int offset) {



memset(HTSMUX_I2CRequest, 0, sizeof(tByteArray));

if (smuxData[link].status != HTSMUX_STAT_NORMAL)

HTSMUXsendCommand(link, HTSMUX_CMD_RUN);
HTSMUX_I2CRequest.arr[0] = 2; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_I2C_BUF + (HTSMUX_BF_ENTRY_SIZE * channel) + offset;
if (!writeI2C(link, HTSMUX_I2CRequest, numbytes))

return false;


if (!readI2C(link, HTSMUX_I2CReply, numbytes))

return false;


memcpy(result, HTSMUX_I2CReply, sizeof(tByteArray));
return true;

}

/**



* Read the value returned by the sensor attached the SMUX. This

* function is for I2C sensors.

*

* @param link the SMUX port number



* @param channel the SMUX channel number

* @param result array to hold values returned from SMUX

* @param numbytes the size of the I2C reply

* @return true if no error occured, false if it did

*/

bool HTSMUXreadPort(tSensors link, byte channel, tByteArray &result, int numbytes) {



return HTSMUXreadPort(link, channel, result, numbytes, 0);

}

/**



* Read the value returned by the sensor attached the SMUX. This

* function is for analogue sensors.

*

* @param link the SMUX port number



* @param channel the SMUX channel number

* @return the value of the sensor or -1 if an error occurred.

*/

int HTSMUXreadAnalogue(tSensors link, byte channel) {



memset(HTSMUX_I2CRequest, 0, sizeof(tByteArray));

if (smuxData[link].status != HTSMUX_STAT_NORMAL)

HTSMUXsendCommand(link, HTSMUX_CMD_RUN);
if (smuxData[link].sensor[channel] != HTSMUXAnalogue)

return -1;





HTSMUX_I2CRequest.arr[0] = 2; // Message size

HTSMUX_I2CRequest.arr[1] = HTSMUX_I2C_ADDR; // I2C Address

HTSMUX_I2CRequest.arr[2] = HTSMUX_ANALOG + (HTSMUX_AN_ENTRY_SIZE * channel);


if (!writeI2C(link, HTSMUX_I2CRequest, 2))

return -1;


if (!readI2C(link, HTSMUX_I2CReply, 2))

return -1;


return (ubyteToInt(HTSMUX_I2CReply.arr[0]) * 4) + ubyteToInt(HTSMUX_I2CReply.arr[1]);

}

/**



* Return a string for the sensor type.

*

* @param muxsensor the SMUX sensor port number



* @param sensorName the string to hold the name of the sensor.

*/

void HTSMUXsensorTypeToString(HTSMUXSensorType muxsensor, string &sensorName) {



switch(muxsensor) {

case HTSMUXAnalogue: sensorName = "Analogue"; break;

case HTSMUXLegoUS: sensorName = "Ultra Sonic"; break;

case HTSMUXCompass: sensorName = "Compass"; break;

case HTSMUXColor: sensorName = "Colour"; break;

case HTSMUXColorNew: sensorName = "Colour New"; break;

case HTSMUXAccel: sensorName = "Accel"; break;

case HTSMUXIRSeeker: sensorName = "IR Seeker"; break;

case HTSMUXProto: sensorName = "Proto Board"; break;

case HTSMUXIRSeekerNew: sensorName = "IR Seeker V2"; break;

case HTSMUXSensorNone : sensorName = "No sensor"; break;

}

}


/**

* This function returns the smaller of the two numbers

* @param x1 the first number

* @param x2 the second number

* @return the smaller number

*/

int min(int x1, int x2) {



return (x1 < x2) ? x1 : x2;

}

/**



* This function returns the bigger of the two numbers

* @param x1 the first number

* @param x2 the second number

* @return the bigger number

*/

int max(int x1, int x2) {



return (x1 > x2) ? x1 : x2;

}
/**

* This function returns an int equivalent of an unsigned byte

* @param byteVal the byte to be converted

* @return the integer equivalent

*/

int ubyteToInt(byte byteVal) {



return 0x00FF & byteVal;

}




/**


* Returns x if it is between min and max. If outside the range,

* it returns min or max.

* @param x the number to clip

* @param min the minimum value x can have

* @param max the maximum value x can have

* @return x if it is between min and max. If outside the range,

* it returns min or max.

*/
int clip(int x, int min, int max) {

if (x < min)

return min;

else if (x > max)

return max;

else

return x;



}
#endif // __COMMON_H__
/*

* $Id: common.h 17 2009-10-30 11:24:51Z xander $

*/

1   ...   10   11   12   13   14   15   16   17   ...   41


The database is protected by copyright ©dentisty.org 2016
send message

    Main page