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; }