int main(int argc, char** argv) {
(void) argc;
(void) argv;
Aversive::init();
Task t1([]() {
if(enc_l.getValue() >= 10000 && MOT_L >= 0) {
mot_l.setValue(-10);
//io << "stop left !\n";
//Aversive::stop();
}
if(enc_r.getValue() >= 50000 && MOT_R >= 0) {
mot_r.setValue(-10);
//io << "stop right !\n";
}
//ClientThread::instance().sendMessage(ClientThread::INFO, "TEST");
});
t1.setPeriod(10000);
t1.setRepeat();
sched.addTask(t1);
mot_l.setValue(100);
mot_r.setValue(100);
while(Aversive::sync()) {
//io << "test " << ENC_L << " " << ENC_R << "\n";
}
Aversive::setReturnCode(0);
return Aversive::exit();
}
void TurnRight::Handle_Motor(Motor& rightMotor, Motor& leftMotor){
std::cout << "Turning right" << endl;
rightMotor.setValue(1);
leftMotor.setValue(-1);
}
