task main(){
if(!readCalib()) return; //reads calibration informations, return if it fails.
nMotorEncoder[Left]=0;
nMotorEncoder[Right]=0;
//use PID controller to follow the line
PIDDriver(cspeed, threshold + targetCalib, Kp, Kd, Ki);
wait1Msec(10000);
return;
}
task main(){
if(!readCalib()) return; //reads calibration informations, return if it fails.
// initialize the log file
const string logName = "E2Log.txt";
short logSize = 2000;
Delete(logName, logResultIO);
OpenWrite(logHandle, logResultIO,logName, logSize);
//use PID controller to follow the line
PIDDriver(threshold + targetCalib);
Close(logHandle, logResultIO);
return;
}
void Baro::init(I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh)
{
i2c_handle_ = hi2c;
nh_ = nh;
nh_->subscribe<std_msgs::UInt8, Baro>(baro_config_sub_);
BARO_H;
//reset
uint8_t reg[1];
reg[0] = CMD_MS56XX_RESET;
HAL_I2C_Master_Transmit(i2c_handle_, MS561101BA_ADDRESS, reg, 1, 100);
HAL_Delay(10);
uint16_t prom[8];
if (!readCalib(prom)) return;
calibrated_ = true;
// Save factory calibration coefficients
c1_ = prom[1];
c2_ = prom[2];
c3_ = prom[3];
c4_ = prom[4];
c5_ = prom[5];
c6_ = prom[6];
// Send a command to read temperature first
reg[0] = ADDR_CMD_CONVERT_TEMPERATURE;
//reg[0] = ADDR_CMD_CONVERT_PRESSURE;
HAL_I2C_Master_Transmit(i2c_handle_, MS561101BA_ADDRESS, reg, 1, 100);
last_timer_ = HAL_GetTick();
state_ = 0;
s_d1_ = 0;
s_d2_ = 0;
d1_count_ = 0;
d2_count_ = 0;
}