Revision c0757912

View differences:

devices/DiWheelDrive/main.cpp
1025 1025
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
1026 1026
}
1027 1027

  
1028
/**
1029
 * Blocks as long as the position changes.
1030
 */
1031
void checkForMotion(){
1032
  int motion = 1;
1033
  int led = 0;
1034
  types::position oldPos = global.odometry.getPosition();
1035
  while(motion){
1036
    BaseThread::sleep(500);
1037
    types::position tmp = global.odometry.getPosition();
1038
    motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
1039
    oldPos = tmp;
1040
      global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
1041
      global.robot.setLightColor(led % 8, Color(Color::BLACK));
1042
      led++;
1043
  }
1044
}
1045

  
1046
/**
1047
 * Turns MotorControl off and checks if position remains stable.
1048
 */
1049
int checkDockingSuccess(){
1050
  // global.motorcontrol.setTargetRPM(0,0);
1051
  checkForMotion();
1052
  int led = 0;
1053
  int success = 0;
1054
  global.odometry.resetPosition();
1055
  types::position start = global.startPos = global.odometry.getPosition();
1056
  global.motorcontrol.toggleMotorEnable();
1057
  BaseThread::sleep(1000);
1058
  types::position stop = global.endPos = global.odometry.getPosition();
1059
  global.motorcontrol.toggleMotorEnable();
1060
  // Amiro moved, docking was not successful
1061
  if ((start.x + stop.x) || (start.y + stop.y)){
1062
    for(led=0; led<8; led++){
1063
      global.robot.setLightColor(led, Color(Color::RED));
1064
    }
1065
    success = 0;
1066
  }else{
1067
    for(led=0; led<8; led++){
1068
      global.robot.setLightColor(led, Color(Color::GREEN));
1069
    }
1070
    success = 1;
1071
  }
1072
  
1073
  BaseThread::sleep(500);
1074
  for(led=0; led<8; led++){
1075
          global.robot.setLightColor(led, Color(Color::BLACK));
1076
  }
1077
  return success;
1078
}
1079

  
1080
/**
1081
 * Switch to EDGE_LEFT line following and orientate for n steps along the line.
1082
 * 
1083
 * @param lf  current line following
1084
 * @param steps steps to take for the correction (measured in CAN-update durations)
1085
 */
1086
void correctPosition(LineFollow &lf, int steps){
1087
  int checkWhite = 0;
1088
  int rpmSpeed[2] = {0};
1089
  lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
1090
  for (int correction=0; correction<steps; correction++){
1091
    checkWhite = lf.followLine(rpmSpeed);
1092
    setSpeed(rpmSpeed);
1093
    BaseThread::sleep(CAN::UPDATE_PERIOD);
1094
  }
1095
}
1096

  
1097

  
1098

  
1028 1099
void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){
1029 1100
  int steps = 10000;
1030 1101
  int speed = 0;
......
1032 1103
  int led = 0;
1033 1104
  int checkWhite = 0;
1034 1105
  int rpmSpeed[2] = {0};
1035
  int proxyWheelThresh = 13000;
1106
  int proxyWheelThresh = 20000;
1036 1107
  LineFollow lf(&global);
1037 1108
  if (argc == 1){
1038 1109
      chprintf(chp, "%i steps \n", atoi(argv[0]));
......
1048 1119
    BaseThread::sleep(MS2ST(5000));
1049 1120
    global.forwardSpeed = speed;
1050 1121
    
1051
    // rpmSpeed[0] = -speed;
1052
    // rpmSpeed[1] = speed;
1053 1122
    setSpeed(rpmSpeed);
1054
    // lf.setStrategy(LineFollowStrategy::DOCKING);
1123

  
1124

  
1055 1125
    for(int s=0; s < steps; s++){
1056 1126

  
1057 1127
      int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1058 1128
      int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1059 1129
      if ((WL+WR) < proxyWheelThresh){
1130
        global.motorcontrol.setTargetRPM(0,0);
1060 1131
        chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n");
1061 1132
        if(lf.getStrategy() == LineFollowStrategy::DOCKING){
1062
          break;
1133
          // Check if Docking was successful
1134
          if(checkDockingSuccess()){
1135
            break;
1136
          }else{
1137
            correctPosition(lf, 250);
1138
            lf.setStrategy(LineFollowStrategy::DOCKING);
1139
            // break;
1140
          }
1141
          
1063 1142
        }else{
1064
          global.motorcontrol.setTargetRPM(0,0);
1143
          // global.motorcontrol.setTargetRPM(0,0);
1065 1144
          BaseThread::sleep(1000);
1145
          // 180° Rotation 
1066 1146
          global.distcontrol.setTargetPosition(0, 3141592, 10000);
1067
          BaseThread::sleep(10000);
1068
          lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
1069
          for (int correction=0; correction<300; correction++){
1070
            checkWhite = lf.followLine(rpmSpeed);
1071
            setSpeed(rpmSpeed);
1072
            BaseThread::sleep(CAN::UPDATE_PERIOD);
1073
          }
1147
          // BaseThread::sleep(8000);
1148
          checkForMotion();
1149
          correctPosition(lf, 250);
1074 1150
          // break;
1075 1151
          lf.setStrategy(LineFollowStrategy::DOCKING);
1076 1152
          
......
1081 1157
      BaseThread::sleep(CAN::UPDATE_PERIOD);
1082 1158
  }
1083 1159

  
1084
  global.motorcontrol.setTargetRPM(0,0);
1085
  global.odometry.resetPosition();
1086
  global.startPos = global.odometry.getPosition();
1087
  global.motorcontrol.toggleMotorEnable();
1088
  BaseThread::sleep(3000);
1089
  global.endPos = global.odometry.getPosition();
1090
  global.motorcontrol.toggleMotorEnable();
1160
  
1091 1161
}
1092 1162

  
1093 1163

  
......
1130 1200
  types::position pos = global.odometry.getPosition();
1131 1201
  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);
1132 1202
  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);
1203

  
1204
  if(argc == 1){
1205
    for (int i=0; i<atoi(argv[0]);i++){
1206
      types::position pos = global.odometry.getPosition();
1207
      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);
1208
      BaseThread::sleep(CAN::UPDATE_PERIOD);
1209
    }
1210
  }
1133 1211
}
1134 1212

  
1135 1213
static const ShellCommand commands[] = {

Also available in: Unified diff