int ReadGSensor(void)
{
unsigned char d1,d2,d3,d4,d5,d6;
if(i2c0master_StartN(G_SENSOR, I2C_WRITE, 1) == false)
{
printf("Gsensor : %s !!\n",roboio_GetErrMsg());
return false;
}
i2c0master_SetRestartN(I2C_READ, 6);
i2c0master_WriteN(0x32); //Read from X register (Address : 0x32)
/* write stop and start read */
d1 = i2c0master_ReadN(); //X MSB
d2 = i2c0master_ReadN(); //X LSB
d3 = i2c0master_ReadN(); //Y MSB
d4 = i2c0master_ReadN(); //Y LSB
d5 = i2c0master_ReadN(); //Z MSB
d6 = i2c0master_ReadN(); //Z LSB
G_AXIS_VALUE[0] = (d2 & 0x02) ? ~(0xFFFF ^ (d2*256+d1)) : d2*256+d1;
G_AXIS_VALUE[1] = (d4 & 0x02) ? ~(0xFFFF ^ (d4*256+d3)) : d4*256+d3;
G_AXIS_VALUE[2] = (d6 & 0x02) ? ~(0xFFFF ^ (d6*256+d5)) : d6*256+d5;
if(G_AXIS_VALUE[0] == 0 && G_AXIS_VALUE[1] == 0 && G_AXIS_VALUE[2] == 0)
return false;
return true;
}
int InitSensor(void)
{
if (i2c_Init2(0xffff,I2C_USEMODULE0+I2C_USEMODULE1,I2CIRQ_DISABLE,I2CIRQ_DISABLE) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0_SetSpeed(I2CMODE_AUTO, 400000L);
i2c1_SetSpeed(I2CMODE_AUTO, 400000L);
if(i2c0master_StartN(G_SENSOR,I2C_WRITE,2) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0master_WriteN(0x2d); //mode register
i2c0master_WriteN(0x28); //Link and measure mode
delay_ms(100);
if(i2c0master_StartN(G_SENSOR,I2C_WRITE,2) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0master_WriteN(0x31); //mode register
i2c0master_WriteN(0x08); //Full-resolution
delay_ms(100);
if(i2c0master_StartN(G_SENSOR,I2C_WRITE,2) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0master_WriteN(0x38); //mode register
i2c0master_WriteN(0x00); //bypass mode
delay_ms(100);
return true;
}
int main(void) {
unsigned long home[32] = {0L};
unsigned long frame[32] = {0L};
// first set the correct RoBoard version
roboio_SetRBVer(RB_100);
//roboio_SetRBVer(RB_100RD); // if your RoBoard is RB-100RD
//roboio_SetRBVer(RB_110); // if your RoBoard is RB-110
//roboio_SetRBVer(RB_050); // if your RoBoard is RB-050
rcservo_SetServo(RCSERVO_PINS1, RCSERVO_SERVO_DEFAULT_NOFB); // select the servo model on pin S1 as non-feedback servo
rcservo_SetServo(RCSERVO_PINS2, RCSERVO_SERVO_DEFAULT_NOFB); // select the servo model on pin S2 as non-feedback servo
if (rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2) == false) // set PWM/GPIO pins S1 & S2 as Servo mode
{
printf("ERROR: fail to init RC Servo lib (%s)!\n", roboio_GetErrMsg());
return -1;
}
home[0] = home[1] = 1500L; // set the initial home position of all servos as 1500us
rcservo_EnterPlayMode_HOME(home); // enter Action Playing Mode for moving servos
printf("press ENTER to move servo on pins S1 & S2.\n"); getchar();
frame[0] = 900L; // move the servo on pin S1 to position 900us
frame[1] = 1800L; // move the servo on pin S2 to position 1800us
rcservo_MoveTo(frame, 2000L); // move servos to the target positions in 2000ms
printf("press ENTER to move only the servo on pin S1.\n"); getchar();
rcservo_MoveOne(RCSERVO_PINS1, 1800L, 500L); // move the servo to position 1800us in 300ms
printf("press ENTER to move servo on pins S1 & S2.\n"); getchar();
frame[0] = 900L; // move the servo on pin S1 to position 900us
frame[1] = 900L; // move the servo on pin S2 to position 900us
rcservo_MoveTo(frame, 3000L); // move servos to the target positions in 3000ms
printf("Complete.\n");
rcservo_Close(); // close RC Servo lib
return 0;
}
int main(void) {
// first set the correct RoBoard version
roboio_SetRBVer(RB_100);
//roboio_SetRBVer(RB_100RD); // if your RoBoard is RB-100RD
//roboio_SetRBVer(RB_110); // if your RoBoard is RB-110
//roboio_SetRBVer(RB_050); // if your RoBoard is RB-050
if (rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2) == false) // set PWM/GPIO pins S1 & S2 as Servo mode
{
printf("ERROR: fail to init RC Servo lib (%s)!\n", roboio_GetErrMsg());
return -1;
}
rcservo_EnterPWMMode(); // make all servo-mode pins go into PWM mode
printf("Send PWM pulses of period 10ms duty 1500us on pin S1...\n");
rcservo_SendCPWM(RCSERVO_PINS1, 10000L, 1500L);
printf("Send PWM pulses of period 5ms duty 800us on pin S2...\n");
rcservo_SendCPWM(RCSERVO_PINS2, 5000L, 800L);
printf("Press ENTER to change PWM output.\n"); getchar();
printf("Send PWM pulses of period 20ms duty 2300us on pin S1...\n");
rcservo_SendCPWM(RCSERVO_PINS1, 20000L, 2300L);
printf("Send PWM pulses of period 10ms duty 1000us on pin S2...\n");
rcservo_SendCPWM(RCSERVO_PINS2, 10000L, 1000L);
printf("Press ENTER to stop.\n"); getchar();
rcservo_StopPWM(RCSERVO_PINS1);
rcservo_StopPWM(RCSERVO_PINS2);
rcservo_Close(); // close RC Servo lib
return 0;
}
void initRCservo()
{
ServoLibrary::iterator i = servos_.begin();
for(; i != servos_.end(); ++i)
{
//std::string& joint_name = i->first;
Servo& servo = i->second;
if (servo.channel_ >= 1 && servo.channel_ <= 32 && servo.bus_ == Servo::PWM)
{
rcservo_SetServo(servo.channel_ - 1, servo.type_);
if (servo.min_pwm_ != 700 || servo.max_pwm_ != 2300)
rcservo_SetServoParams1(servo.channel_ - 1, 8000L, servo.min_pwm_, servo.max_pwm_);
}
}
if (rcservo_Initialize(servos_.getUsedPWMChannels()) == true)
{
rcservo_EnableMPOS();
rcservo_SetFPS(servo_fps_);
}
else
ROS_ERROR("RoBoIO: %s", roboio_GetErrMsg());
}
int main()
{
unsigned long usedchannel = RCSERVO_USECHANNEL0 +RCSERVO_USECHANNEL1 +RCSERVO_USECHANNEL2 +
RCSERVO_USECHANNEL4 +RCSERVO_USECHANNEL5 +RCSERVO_USECHANNEL6+
RCSERVO_USECHANNEL9 +RCSERVO_USECHANNEL10+RCSERVO_USECHANNEL11+
RCSERVO_USECHANNEL13+RCSERVO_USECHANNEL14+RCSERVO_USECHANNEL15+
RCSERVO_USECHANNEL16+RCSERVO_USECHANNEL17+RCSERVO_USECHANNEL18+
RCSERVO_USECHANNEL21+RCSERVO_USECHANNEL22+RCSERVO_USECHANNEL23; // for RB-100
if(allegro_init())
{
printf("error:initialize allegro library failed!!\n");
return -1;
}
if(install_keyboard())
{
printf("error:initialize keybaord failed!!\n");
return -1;
}
roboio_SetRBVer(RB_100);
if(rcservo_SetServos(usedchannel, RCSERVO_DMP_RS0263) == false)
{
printf("Set servo fails!%s\n",roboio_GetErrMsg());
return -1;
}
if(rcservo_Initialize(usedchannel) == false)
{
printf("RC servo initialize fails!%s\n",roboio_GetErrMsg());
return -1;
}
rcservo_SetFPS(500);
rcservo_EnterPlayMode_NOFB(NORMAL);
SetVal();
PRE_STATE = STATE = BALANCE;
ADJUST = 0.0;
BODY_YAW = 0.0;
BODY_PITCH = 0.0;
BODY_ROLL = 0.0;
POS_GAIN[0] = 0.0;
POS_GAIN[1] = 0.0;
POS_GAIN[2] = 0.0;
Prepare();
while(STATE != EXIT)
{
GetInput();
switch(STATE)
{
case FORWARD:
case BACKWARD:
case RIGHTWARD:
case LEFTWARD:
case FLWARD:
case FRWARD:
case BLWARD:
case BRWARD:
case LCIRCLE:
case RCIRCLE:
PlayMotion();
break;
case IDLE:
Idle();
break;
case BALANCE:
Stable();
break;
default:
break;
}
}
rcservo_Close();
i2c_Close();
remove_keyboard();
return 0;
}
//------------------------------------------------------------------------------
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
MotorDriver::MotorDriver( ConfigFile* pConfigFile, int section )
: ThreadedDriver( pConfigFile, section, false,
PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION3D_CODE ),
mbCompassAvailable( false ),
mLastDisplayedCompassAngleTimestamp( 0.0 ),
mbDepthSensorAvailable( false ),
mLastDisplayedDepthSensorDepthTimestamp( 0.0 ),
mbInitialisedPWM( false )
{
this->alwayson = true;
if ( !pwm_Initialize( 0xffff, PWMCLOCK_50MHZ, PWMIRQ_DISABLE ) )
{
printf( "Unable to initialise PWM library - %s\n", roboio_GetErrMsg() );
mbInitialisedPWM = false;
}
else
{
printf( "PWM library initialised\n" );
mbInitialisedPWM = true;
// Set the channels to produce a zero velocity PWM
pwm_SetPulse( LEFT_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + LEFT_PWM_OFFSET );
pwm_SetPulse( RIGHT_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + RIGHT_PWM_OFFSET );
pwm_SetPulse( FRONT_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + FRONT_PWM_OFFSET );
pwm_SetPulse( BACK_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + BACK_PWM_OFFSET );
pwm_SetPulse( TEST_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US );
pwm_SetCountingMode( LEFT_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( RIGHT_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( FRONT_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( BACK_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( TEST_CHANNEL, PWM_CONTINUE_MODE );
// Enable the pins
pwm_EnablePin( LEFT_MOTOR_CHANNEL );
pwm_EnablePin( FRONT_MOTOR_CHANNEL );
pwm_EnablePin( RIGHT_MOTOR_CHANNEL );
pwm_EnablePin( BACK_MOTOR_CHANNEL );
pwm_EnablePin( TEST_CHANNEL );
printf( "All go\n" );
//printf( "Outputting many pulses\n" );
pwm_EnableMultiPWM(0xffffffffL);
printf( "Enabled everything\n" );
}
mpCompass = NULL;
// See if we have an imu (compass) device
if ( pConfigFile->ReadDeviceAddr( &mCompassID, section, "requires",
PLAYER_IMU_CODE, -1, NULL ) != 0 )
{
PLAYER_WARN( "No Compass driver specified" );
}
else
{
mbCompassAvailable = true;
}
mpDepthSensor = NULL;
// See if we have a depth sensor
if ( pConfigFile->ReadDeviceAddr( &mDepthSensorID, section, "requires",
PLAYER_POSITION1D_CODE, -1, NULL ) != 0 )
{
PLAYER_WARN( "No Depth Sensor specified" );
}
else
{
mbDepthSensorAvailable = true;
}
}
void Altimeter_Sensor::read()
{
//read SHT21
i2c0master_StartN(SHT21_ADDR, I2C_WRITE,1);
i2c0master_WriteN(0xf5);
//delay_ms(100);
//wait_ms(100);
i2c0master_StartN(SHT21_ADDR, I2C_READ,3);
sht21_humi[1] = i2c0master_ReadN();
sht21_humi[0] = i2c0master_ReadN();
// checksum
i2c0master_ReadN();
i2c0master_StartN(SHT21_ADDR, I2C_WRITE,1);
i2c0master_WriteN(0xf3);
//wait_ms(100);
i2c0master_StartN(SHT21_ADDR, I2C_READ,3);
sht21_temp[1] = i2c0master_ReadN();
sht21_temp[0] = i2c0master_ReadN();
// checksum
i2c0master_ReadN();
// LPS331AP
// read LPS331AP
i2c_SensorRead(LPS331AP_ADDR, 0x27, &lps331ap_state,1);
//wait_ms(100);
if(lps331ap_state == 0xff)
{
printf("LPS331AP error:%s !!\n",roboio_GetErrMsg());
}
// check error
if(lps331ap_state& 0x02)
{
if(i2c_SensorRead(LPS331AP_ADDR, 0x28, &lps331ap_press[0],1) == false)
{
printf("LPS331AP fail to read pressure XLB (%s)!\n", roboio_GetErrMsg());
}
if(i2c_SensorRead(LPS331AP_ADDR, 0x29, &lps331ap_press[1],1) == false)
{
printf("LPS331AP fail to read pressure LB (%s)!\n", roboio_GetErrMsg());
}
if(i2c_SensorRead(LPS331AP_ADDR, 0x2a, &lps331ap_press[2],1) == false)
{
printf("LPS331AP fail to read pressure MSB (%s)!\n", roboio_GetErrMsg());
}
}
if(lps331ap_state& 0x01)
{
if(i2c_SensorRead(LPS331AP_ADDR, 0x2b, lps331ap_temp,1) == false)
{
printf("LPS331AP fail to read temp LSB (%s)!\n", roboio_GetErrMsg());
}
if(i2c_SensorRead(LPS331AP_ADDR, 0x2c, lps331ap_temp+1,1) == false)
{
printf("LPS331AP fail to read temp MSB (%s)!\n", roboio_GetErrMsg());
}
}
// SHT21
sht21_rh = (double) (((long)sht21_humi[1]<< 8) + (long)sht21_humi[0]);
sht21_rh = sht21_rh*125/65536 - 6;
//printf("== SHT21 ==\n\nhumidity:%f RH ",sht21_rh);
sht21_deg = (double) (((long)sht21_temp[1]<< 8L) + (long)sht21_temp[0]);
sht21_deg = sht21_deg*175.72/65535 - 46.85;
//printf("temp:%f deg\n\n",sht21_deg);
//LPS331AP
lps331ap_mbar = (double) ((((long)lps331ap_press[2])<<16L) + (((long)lps331ap_press[1])<< 8L) + (long)lps331ap_press[0]);
lps331ap_mbar = lps331ap_mbar/4096;
//printf("== LPS331AP ==\n\npressure:%f mbar ",lps331ap_mbar);
/**************************************************
*
PSVL = P*10^M
M = StationHigh/(18400*(1+Temp/273))
*
**************************************************/
lps331ap_deg = (double) ((((int)lps331ap_temp[1])<< 8L) + (int)lps331ap_temp[0]);
lps331ap_deg = 42.5 + lps331ap_deg/480;
//printf("temp:%f deg\n\n",lps331ap_deg);
height = log10(1013.25/lps331ap_mbar)*8400*(1+lps331ap_deg/273.0);
//printf("height:%f",height);
rawHeightFromSensor=(int)height;
}
