AMiRo-OS

void MotorControl::setGains(motorGains *motorConfig){

  chSysLock();

  this->pGain = motorConfig->pGain;

  this->iGain = motorConfig->iGain;

  this->dGain = motorConfig->dGain;

  chSysUnlock();

}

void MotorControl::getGains(motorGains *motorConfig){

  motorGains gains;

  motorConfig->pGain = this->pGain;

  motorConfig->iGain = this->iGain;

  motorConfig->dGain = this->dGain;

}

void MotorControl::toggleMotorEnable(){

  this->accumulatedErrorV = 0;

  this->accumulatedErrorW = 0;

  this->motorEnable = !this->motorEnable;

}

void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {

void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {

  global.motorcontrol.getGains(&global.motorConfigGains);

  global.motorcontrol.setGains(&global.stopGains);

  global.motorcontrol.setGains(&global.motorConfigGains);

// Either 0 to disable record or > 0 to enable it

void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){

  if (argc == 1){

    chprintf(chp, "Set recording to %d\n", atoi(argv[0]));

    global.enableRecord = atoi(argv[0]);

  }

}

void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {

  // int vcnl4020AmbientLight[4];

  // int vcnl4020Proximity[4];

  int rpmSpeed[2] = {0};

  int steps = 0;

  // int proxyL = global.threshProxyL;

  // int proxyR = global.threshProxyR;

  int maxDelta = 0;

  float KCrit = 0.0f;

  global.sensSamples = 0;

  global.maxDist.error = 0;

  LineFollow lf(&global);

  int led = 0;

  if (argc == 2){

    chprintf(chp, "KCrti %f\n", atof(argv[0]));

    chprintf(chp, "Steps %i\n", atoi(argv[1]));

    KCrit = atof(argv[0]);

    steps = atoi(argv[1]);

  } else if (argc == 3){

    chprintf(chp, "KCrti %f\n", atof(argv[0]));

    chprintf(chp, "Steps %i\n", atoi(argv[1]));

    global.K_p = atof(argv[0]);

    global.K_d = atof(argv[1]);

    steps = atoi(argv[2]);

    global.forwardSpeed = 15;

  }else{

    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <kd> <steps> (<speed>)");

    return;

  }

  for(led=0; led<8; led++){

        global.robot.setLightColor(led, Color(Color::BLACK));

  }

  chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");

  BaseThread::sleep(MS2ST(5000));

  // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);

  int checkWhite = 0;

  int it_switch = steps / 2;

  // lf.setStrategie(LineFollowStrategie::MIDDLE);

  for(int s=0; s < steps; s++){

    checkWhite = lf.followLine(rpmSpeed);

    checkWhite = 0;

    // chprintf(chp,"S:%d,",s);

    // if(global.threshWhite)

    // if(s < it_switch){

    //   lf.setStrategie(LineFollowStrategie::EDGE_LEFT);

    //   checkWhite = lf.followLine(rpmSpeed);

    // }else{

    //   lf.setStrategie(LineFollowStrategie::EDGE_RIGHT);

    //   checkWhite = lf.followLine(rpmSpeed);

    // }

    if(checkWhite){

      global.motorcontrol.setTargetRPM(0,0);

      for(led=0; led<8; led++){

        global.robot.setLightColor(led, Color(Color::RED));

      }

    }else{

      global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);

    }

    BaseThread::sleep(CAN::UPDATE_PERIOD);

  }

  global.motorcontrol.setTargetRPM(0,0);

}

void followLine(BaseSequentialStream *chp, int argc, char *argv[]){

  int steps = 1000;

  int speed = 0;

  int strategy = 0;

  int led = 0;

  int checkWhite = 0;

  int rpmSpeed[2] = {0};

  LineFollow lf(&global);

  if (argc == 1){

      chprintf(chp, "%i steps \n", atoi(argv[0]));

      steps = atoi(argv[0]);

    }else if (argc == 2){

      steps = atoi(argv[0]);

      speed = atoi(argv[1]);

    }else if (argc == 3){

      steps = atoi(argv[0]);

      speed = atoi(argv[1]);

      strategy = atoi(argv[2]);

    }else{

      chprintf(chp, "Use: followLine <steps> <speed> <strategy>\n");

      return;

    }

    global.forwardSpeed = speed;

    switch (strategy)

    {

    case 0:

      lf.setStrategy(amiro::LineFollowStrategy::EDGE_RIGHT);

      break;

    case 1:

      lf.setStrategy(amiro::LineFollowStrategy::EDGE_LEFT);

      break;

    case 2:

      lf.setStrategy(amiro::LineFollowStrategy::FUZZY);

      break;

    default:

      break;

    }

    for(int s=0; s < steps; s++){

      checkWhite = lf.followLine(rpmSpeed);

      if(checkWhite){

        global.motorcontrol.setTargetRPM(0,0);

        for(led=0; led<8; led++){

          global.robot.setLightColor(led, Color(Color::RED));

        }

      }else{

        global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);

      }

      BaseThread::sleep(CAN::UPDATE_PERIOD);

  }

  global.motorcontrol.setTargetRPM(0,0);

}

void printMove(BaseSequentialStream *chp, int argc, char *argv[]){

  for (int j=0; j<global.sensSamples;j++){

    chprintf(chp,"FL:%d,FR:%d,Delta:%d,Error:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta, global.senseRec[j].error);

  }

  chprintf(chp,"MaxDist: FL:%d,FR:%d,Delta:%d,Error:%d\n",global.maxDist.FL, global.maxDist.FR, global.maxDist.delta, global.maxDist.error);

}

void freeGains(BaseSequentialStream *chp, int argc, char *argv[]){

  if (global.resetProtect){

    global.motorcontrol.getGains(&global.motorConfigGains);

    global.resetProtect = 0;

  }

  global.motorcontrol.setGains(&global.stopGains);

}

void setGains(BaseSequentialStream *chp, int argc, char *argv[]){

  if(!global.resetProtect){

    global.motorcontrol.setGains(&global.motorConfigGains);

    global.resetProtect = 1;

  }

}

void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){

  global.odometry.resetPosition();

  global.motorcontrol.toggleMotorEnable();

}

void setSpeed(int (&rpmSpeed)[2]){

  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);

}

void enableCharging(){

  global.ltc4412.enable(true);

}

void disableCharging(){

  global.ltc4412.enable(false);

}

void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){

  uint8_t strategy = 0;

  if(argc == 1){

    strategy = atoi(argv[0]); 

  }

  // send over can

  global.strategyTest = strategy;

  global.triggerCan = true;

}

void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){

  types::position pos = global.odometry.getPosition();

  chprintf(chp, "Start: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z);

  chprintf(chp, "End: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.endPos.x, global.endPos.y, global.endPos.f_x, global.endPos.f_y, global.endPos.f_z);

  if(argc == 1){

    for (int i=0; i<atoi(argv[0]);i++){

      types::position pos = global.odometry.getPosition();

      chprintf(chp, "End: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z);

      BaseThread::sleep(CAN::UPDATE_PERIOD);

    }

  }

}

// TODO: Not wokring, either loading station has no power or logic not working

void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){

  // enableCharging();

  // for (int i=0; i<steps;i++){

  //   chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); 

  //   if(global.ltc4412.isPluggedIn()){

  //     // enableCharging();

  //     for(led=0; led<8; led++){

  //       global.robot.setLightColor(led, Color(Color::GREEN));

  //     }

  //   }else{

  //     // disableCharging();

  //     for(led=0; led<8; led++){

  //       global.robot.setLightColor(led, Color(Color::RED));

  //     }

  //   }

  //   BaseThread::sleep(CAN::UPDATE_PERIOD);

  // }

  // disableCharging();

  // for(led=0; led<8; led++){

  //   global.robot.setLightColor(led, Color(Color::BLACK));

  // }

  // chprintf((BaseSequentialStream*) &SD1, "Charging Flags: %d, State of charge: %d, Minutes: %d, Consumption: %d\n", global.powerFrame.data8[0], global.powerFrame.data8[1], global.powerFrame.data16[1], global.powerFrame.data16[2]);

void proxyRing(BaseSequentialStream *chp, int argc, char *argv[]){

  {"motor_deactivate", freeGains},

  {"motor_activate", setGains},

  {"motorToggle", motorToggle},

  {"dev_proxi_sensor_data", getProxySensorData},

  {"dev_ziegler2", zieglerMeth2},

  // TODO: Stop user process from execution to finish/force calibration before anything starts

  {"calibrate_line", calibrateLineSensores}, 

  // {"record_move", recordMove},

  {"print_record", printMove},

  {"setRecord", setRecord},

  {"followLine", followLine},

  {"getPos", getPosition},

  {"checkPower", checkPower},

  {"setStrategy", setGlobalStrategy},

  {"proxyRing", proxyRing},

  this->powerStatus.charging_flags.content.powermanagement_plugged_in = global.ltc4412.isPluggedIn();

  // this->powerStatus.charging_flags.content.powermanagement_plugged_in = (palReadPad((GPIO_TypeDef*)GPIOC, GPIOC_PATHDC) == PAL_HIGH); 

      // current_state = next_state;

      current_state = IDLE;

      next_state = IDLE;

  // const uint32_t UPDATE_PERIOD        = US2ST(62500);  // 16 Hz

  // const uint32_t UPDATE_PERIOD        = US2ST(15625);  // 64 Hz

  // const uint32_t UPDATE_PERIOD        = US2ST(8000);  // 125 Hz

    void setGains(motorGains *motorConfig);

    void getGains(motorGains *motorConfig);

    void toggleMotorEnable();