void rotaryDriverStartCM(uint16_t cm) {
rotaryDriverReset();
set_forward(MOTOR_DEFAULT_TURN_SPEED, MOTOR_DEFAULT_TURN_SPEED);
MP._rotary_driver_state |= ROTARY_DRIVER_ACTIVE;
MP.rotary_driver_target_ticks = 9.71*(cm)-37.47;
setChange(1);
}
void rotaryDriverStart(uint16_t ticks) {
rotaryDriverReset();
set_forward(MP._originalLspeed, MP._originalLspeed);
MP._rotary_driver_state |= ROTARY_DRIVER_ACTIVE;
MP.rotary_driver_target_ticks = ticks;
setChange(1);
}
/**
* Description not yet available.
* \param
*/
void fixed_smartlist2::initialize(void)
{
end_saved=0;
eof_flag=0;
bptr=buffer;
//int nbytes=0;
written_flag=0;
lseek(fp,0L,SEEK_SET);
set_forward();
}
void orderStartForward(void) {
setPIDValue(getOrderLengthToWall()+IR_SENSOR_OFFSET);
resetPIDIntergrator();
movePlatformCM(MP.order_length);
//send_xy(toArray(MP.x_pos), toArray(MP.y_pos));
if(isPlatformMovingIntoRightWall()) {
setOrderTargetTicks(getOrderTargetTicks()-cmtoticks(7));
} else if (isPlatformMovingIntoLeftWall()) {
setOrderTargetTicks(getOrderTargetTicks()-cmtoticks(8));
} else if (isPlatformMovingIntoUpperWall()) {
setOrderTargetTicks(getOrderTargetTicks()+cmtoticks(9));
} else if (isPlatformMovingIntoLowerWall()) {
setOrderTargetTicks(getOrderTargetTicks()-cmtoticks(3));
}
if(getOrderTargetTicks() >= cmtoticks(PLATFORM_PID_THRESHOLD)) {
if(getOrderLengthToWall() < 8.0) {
setPIDSmall();
} else {
setPIDWide();
}
activePID();
} else {
deactivatePID();
}
if(getOrderLengthToWall() < 8.0 || MP.order_length < 30) {
set_forward(MOTOR_REDUCED_SPEED, MOTOR_REDUCED_SPEED);
setOrderTargetTicks(getOrderTargetTicks()+cmtoticks(5));
} else {
set_forward(MOTOR_DEFAULT_SPEED, MOTOR_DEFAULT_SPEED);
}
//activateRotary();
setOrderActive();
}
/* Cheney collector */
static void gc_copy_forward(Ptr *p)
{
unsigned int tag = TAG(*p);
if (tag == number_tag || tag == immed_tag || tag == float_tag) { return; } /* number or immediate or float -> ignore */
Ptr *obj = OBJ(*p);
if (obj == 0) { return; } /* null pointer -> ignore */
if (forwarded(obj)) { /* update pointer with forwarding info */
LOG("U%x", tag);
*p = UNTAG(*obj) | tag;
} else { /* copy and forward object */
LOG("C%x", tag);
unsigned int size = object_size(obj);
memcpy(gc_free, obj, size);
set_forward(obj, PTR(gc_free));
*p = PTR(gc_free) | tag;
gc_free += size;
}
}
Frame::Frame(Vector3 position, Vector3 forward, Vector3 up)
{
memcpy(this->position, position, sizeof(float) * 3);
set_forward(forward);
set_up(up);
}
void FlyingCamera::roll(GLfloat angle) {
glm::fquat quaternion = create_quaternion(get_forward_dir(),angle);
set_up(quaternion*get_up());
set_forward(quaternion * get_forward());
}
void FlyingCamera::pitch(GLfloat angle) {
glm::fquat quaternion = create_quaternion(glm::cross(get_forward_dir(),get_up_dir()),angle);
set_forward(quaternion*get_forward());
set_up(quaternion*get_up());
}
virtual void set_right(const vec3& new_right) override {
set_forward(glm::cross(up(), new_right));
}
void process_platform() {
if(getOrderID() != MP.completed_order) {
if(!(getOrderState()&ORDER_ACTIVE)) {
switch(getOrderType()) {
case(ORDER_TYPE_FORWARD):
orderStartForward();
break;
case(ORDER_TYPE_LEFT_TURN):
orderStartLeftTurn();
break;
case(ORDER_TYPE_RIGHT_TURN):
orderStartRightTurn();
break;
}
} else {
if(MP._rotary_driver_state&ROTARY_DRIVER_ACTIVE) {
if(rotaryDriverDone()) {
deactivatePID();
set_stop();
setOrderDone();
MP.completed_order = getOrderID();
rotaryDriverStop();
rotaryDriverReset();
MP.adjust_state = 0;
}
} else {
if(MP._state == PLATFORM_FORWARD) {
if(getOrderTargetTicks() < cmtoticks(PLATFORM_PID_THRESHOLD)) {
deactivatePID();
//set_forward(MOTOR_DEFAULT_SPEED, MOTOR_DEFAULT_SPEED);
if(getOrderCurrentTicks() >= getOrderTargetTicks()) {
set_stop();
setOrderDone();
MP.completed_order = getOrderID();
MP.adjust_state = 0;
}
} else {
if(!MP.adjust_state&PLATFORM_ADJUST_DONE) {
if(getOrderLengthToWall() < 7.0) {
if(getOrderCurrentTicks() >= getOrderTargetTicks()*0.40 && getOrderCurrentTicks() <= getOrderTargetTicks()*0.60) {
platformFineAdjust();
}
}
}
if(getOrderCurrentTicks() >= getOrderTargetTicks()*0.6) {
if(!checkFrontRight()) {
deactivatePID();
set_forward(MP._originalLspeed, MP._originalRspeed);
}
if((!checkBackRight() && !checkFrontRight()) || (!checkBackLeft() && !checkFrontLeft())) {
set_stop();
deactivatePID();
if(getOrderLengthToWall() >= 7.0) {
rotaryDriverStartCM(8);
} else {
rotaryDriverStartCM(10);
}
}
}
}
} else if(MP._state == PLATFORM_LEFT || MP._state == PLATFORM_RIGHT) {
deactivatePID();
if(getOrderCurrentTicks() >= getOrderTargetTicks()) {
set_stop();
setOrderDone();
MP.completed_order = getOrderID();
}
}
}
}
}
if(change==1){
switch(MP._state){
case(PLATFORM_STOP):
_stop();
break;
case(PLATFORM_FORWARD):
_go_forward();
break;
case(PLATFORM_BACKWARD):
_go_backward();
break;
case(PLATFORM_LEFT):
_turn_left();
break;
case(PLATFORM_RIGHT):
_turn_right();
break;
default:
_stop();
break;
}
change=0;
}
}
