xpcc::co::Result<bool>
task::Mechanics::calibrateZ()
{
CO_BEGIN();
startMotors();
zMotor.stop();
if (buttons.isZ_AxisLimitPressed())
{
zMotor.rotateBy(100, 500);
CO_WAIT_WHILE(zMotor.isRunning());
zMotor.stop();
}
zMotor.setSpeed(-200);
timeout.restart(2000);
CO_WAIT_UNTIL(buttons.isZ_AxisLimitPressed() || timeout.isExpired());
zMotor.stop();
if ( (isCalibratedZ = !timeout.isExpired()) )
{
zMotor.rotateBy(correctionZ, 300);
CO_WAIT_WHILE(zMotor.isRunning());
leds.resetMechanicalError();
leds.resetBusy();
CO_RETURN(true);
}
leds.setMechanicalError();
leds.resetBusy();
releaseMotors();
CO_END_RETURN(false);
}
int main(int argc, char *argv[]) {
pthread_t p_viconThread, p_sendCmmdThread, p_publishCmmdThread, p_fetchDataThread;
pthread_t p_acithread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetCmdListUpdateFinishedCallback(&cmdListUpdateFinished);
aciSetParamListUpdateFinishedCallback(¶mListUpdateFinished);
aciSetEngineRate(100, 10);
lcm = lcm_create(NULL);
if(!lcm)
return 1;
pthread_create(&p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
while(!var_getted) usleep(1000);
aciGetDeviceCommandsList();
printf("Waiting for command list...\n");
while(!cmd_ready) usleep(1000);
printf("starting motors!\n");
//turn on motors()
startMotors();
usleep(1000000);
//start getting data from Vicon
pthread_create(&p_viconThread, NULL, viconStateThread, NULL);
pthread_create(&p_sendCmmdThread, NULL, sendCmmdThread, NULL);
// pthread_create(&p_fetchDataThread, NULL, fetchDataThread, NULL);
pthread_create(&p_publishCmmdThread, NULL, publishCmmdThread, NULL);
pthread_join(p_viconThread, NULL);
pthread_join(p_sendCmmdThread, NULL);
// pthread_join(p_fetchDataThread, NULL);
pthread_join(p_publishCmmdThread, NULL);
pthread_join(p_acithread, NULL);
}
xpcc::co::Result<bool>
task::Mechanics::rotateBackward()
{
CO_BEGIN();
startMotors();
zMotor.rotateBy(-3600, 5000);
CO_WAIT_WHILE(zMotor.isRunning());
xMotor.rotateBy(3600, 5000);
CO_WAIT_WHILE(xMotor.isRunning());
releaseMotors();
CO_END_RETURN(true);
}
void examineID(msg_pointer mp){
printf("ID is %d\n", mp->ID);
if (mp->ID == START_ID)
startMotors();
if (mp->ID == STOP_ID)
stopMotors();
if (mp-> ID == CONTROL_ID)
controlMotors(mp);
if (mp-> ID == SPECIAL_COMMAND_ID)
specialMotorCommand(mp);
return;
}
xpcc::co::Result<bool>
task::Mechanics::rotateForward()
{
CO_BEGIN();
if (!isCalibrated())
CO_RETURN(false);
startMotors();
xMotor.rotateBy(-3600, 10000);
CO_WAIT_WHILE(xMotor.isRunning());
zMotor.rotateBy(3600, 10000);
CO_WAIT_WHILE(zMotor.isRunning());
leds.resetBusy();
isCalibratedX = false;
isCalibratedZ = false;
CO_END_RETURN(true);
}
bool AsctecProc::setMotorsOnOff(mav_srvs::SetMotorsOnOff::Request &req,
mav_srvs::SetMotorsOnOff::Response &res)
{
state_mutex_.lock();
engaging_ = true;
if (req.on && !motors_on_)
{
ctrl_roll_ = 0;
ctrl_pitch_ = 0;
ctrl_yaw_ = 0;
ctrl_thrust_ = 0;
startMotors();
}
else
{
stopMotors();
}
engaging_ = false;
state_mutex_.unlock();
return (req.on == motors_on_);
}
//******************find the exit hatch**************************//
void findEnd(){
int left;
int right;
Servo1SetPos(9);
delay(25);
Servo2SetPos(9);
ADC0_SC1A=ADC_SC1_ADCH(7);
delay(25);
left=DistanceMeasuring1(ADC0_RA);
ADC0_SC1A=ADC_SC1_ADCH(6);
delay(25);
right=DistanceMeasuring2(ADC0_RA);
//checks if free in front
if(left>130&&right>130){
strightTicks=3000;
startMotors(9375);
state=5;
return;
}
//Ir scan to find the hole
IrScanEnd();
int start=8;
int end=79;
//find the atart and end index that the range between the will include only the wall
while(IrSampels[start]>130) start++;
while(IrSampels[end]>130) end--;
int max=0;
int maxIndex=0;
int i;
//clean noises
for(i=start;i<=end;i++){
if ((IrSampels[i+1]>140)&&(IrSampels[i-1]>140)){
IrSampels[i]=150;
}
if ((IrSampels[i+1]<130)&&(IrSampels[i-1]<130)){
IrSampels[i]=20;
}
}
//find the hole
for(i=start;i<=end;i++){
if (max<=IrSampels[i]){
max=IrSampels[i];
maxIndex=i;
}
}
//do optimisation to the center of the hole
int counterLeft=1;
while(max-5<=IrSampels[maxIndex-counterLeft]){
counterLeft++;
}
int counterRight=1;
while(max-5<=IrSampels[maxIndex+counterRight]){
counterRight++;
}
double deg;
//calculate the angle- convert the servos angle to the cars angle
int avrIndex=(counterRight+counterLeft)/2+maxIndex-counterLeft;
if(avrIndex<43){
deg=-90+57.3*atan((cos(0.01745*(43-avrIndex)*2.51)*IrSampels[avrIndex]/5+3)/(0.5+sin(0.01745*(43-avrIndex)*2.51)*IrSampels[avrIndex]/5));
}
else {
deg=90-57.3*atan((cos(0.01745*(avrIndex-44)*2.27)*IrSampels[avrIndex]/5+3)/(0.5+sin(0.01745*(avrIndex-44)*2.27)*IrSampels[avrIndex]/5));
}
int index=0;
//start driving in the desirable direction
turnPivot((int)deg);
strightTicks=3000;
startMotors(9375);
state=5;
}