Revision bfffb0bd devices/DiWheelDrive/main.cpp

View differences:

devices/DiWheelDrive/main.cpp
783 783
    rounds = atoi(argv[0]);
784 784
    
785 785
  }
786
 
786
  global.motorcontrol.getGains(&global.motorConfigGains);
787
  global.motorcontrol.setGains(&global.stopGains);
787 788

  
788 789
  for (int j = 0; j < rounds; j++) {
789 790
    for (int i = 0; i < 4; i++) {
......
813 814
    // sleep(CAN::UPDATE_PERIOD);
814 815
    BaseThread::sleep(CAN::UPDATE_PERIOD);
815 816
  }
817
  global.motorcontrol.setGains(&global.motorConfigGains);
816 818
  // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
817 819
  return;
818 820
}
......
962 964

  
963 965

  
964 966
}
967
void freeGains(BaseSequentialStream *chp, int argc, char *argv[]){
968
  if (global.resetProtect){
969
    global.motorcontrol.getGains(&global.motorConfigGains);
970
    global.resetProtect = 0;
971
  }
972
  
973
  global.motorcontrol.setGains(&global.stopGains);
974
}
975

  
976

  
977
void setGains(BaseSequentialStream *chp, int argc, char *argv[]){
978
  if(!global.resetProtect){
979
    global.motorcontrol.setGains(&global.motorConfigGains);
980
    global.resetProtect = 1;
981
  }
982
}
983
void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){
984
  global.odometry.resetPosition();
985
  global.motorcontrol.toggleMotorEnable();
986
}
987

  
988

  
989
void rotate(BaseSequentialStream *chp, int argc, char *argv[]){
990
  int steps = 1000;
991
  int speed = 0;
992
  int strategy = 0;
993
  int led = 0;
994
  int checkWhite = 0;
995
  int rpmSpeed[2] = {0};
996
  LineFollow lf(&global);
997
  if (argc == 1){
998
      chprintf(chp, "%i steps \n", atoi(argv[0]));
999
      speed = atoi(argv[0]);
1000
    }else if (argc == 2){
1001
      speed = atoi(argv[0]);
1002
      steps = atoi(argv[1]);
1003
    }else{
1004
      chprintf(chp, "Use: rotate <speed> <steps>\n");
1005
      return;
1006
    }
1007
    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
1008
    BaseThread::sleep(MS2ST(5000));
1009
    // global.forwardSpeed = speed;
1010
  //   rpmSpeed[0] = -speed;
1011
  //   rpmSpeed[1] = speed;
1012
  //   global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
1013
  //   for(int s=0; s < steps; s++){
1014
  //     BaseThread::sleep(CAN::UPDATE_PERIOD);
1015
  // }
1016
  global.odometry.resetPosition();
1017
  global.startPos = global.odometry.getPosition();
1018
  global.distcontrol.setTargetPosition(0, 3141592, 5000);
1019
  BaseThread::sleep(MS2ST(11000));
1020
  global.endPos = global.odometry.getPosition();
1021
  // global.motorcontrol.setTargetRPM(0,0);
1022
}
1023

  
1024
void setSpeed(int (&rpmSpeed)[2]){
1025
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
1026
}
1027

  
1028
void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){
1029
  int steps = 10000;
1030
  int speed = 0;
1031
  int strategy = 0;
1032
  int led = 0;
1033
  int checkWhite = 0;
1034
  int rpmSpeed[2] = {0};
1035
  int proxyWheelThresh = 13000;
1036
  LineFollow lf(&global);
1037
  if (argc == 1){
1038
      chprintf(chp, "%i steps \n", atoi(argv[0]));
1039
      speed = atoi(argv[0]);
1040
    }else if (argc == 2){
1041
      speed = atoi(argv[0]);
1042
      steps = atoi(argv[1]);
1043
    }else{
1044
      chprintf(chp, "Use: rotate <speed> <steps>\n");
1045
      return;
1046
    }
1047
    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
1048
    BaseThread::sleep(MS2ST(5000));
1049
    global.forwardSpeed = speed;
1050
    
1051
    // rpmSpeed[0] = -speed;
1052
    // rpmSpeed[1] = speed;
1053
    setSpeed(rpmSpeed);
1054
    // lf.setStrategy(LineFollowStrategy::DOCKING);
1055
    for(int s=0; s < steps; s++){
1056

  
1057
      int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1058
      int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1059
      if ((WL+WR) < proxyWheelThresh){
1060
        chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n");
1061
        if(lf.getStrategy() == LineFollowStrategy::DOCKING){
1062
          break;
1063
        }else{
1064
          global.motorcontrol.setTargetRPM(0,0);
1065
          BaseThread::sleep(1000);
1066
          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
          }
1074
          // break;
1075
          lf.setStrategy(LineFollowStrategy::DOCKING);
1076
          
1077
        }
1078
      }
1079
      checkWhite = lf.followLine(rpmSpeed);
1080
      setSpeed(rpmSpeed);
1081
      BaseThread::sleep(CAN::UPDATE_PERIOD);
1082
  }
1083

  
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();
1091
}
1092

  
1093

  
1094
void getAccel(BaseSequentialStream *chp, int argc, char *argv[]){
1095
  int steps = 0;
1096
  // global.motorcontrol.getGains(&global.motorConfigGains);
1097
  // global.motorcontrol.setGains(&global.stopGains);
1098
  int16_t time = 10000;
1099
  int32_t angle = 3141592;
1100
  int32_t distance = 0;
1101
  if (argc == 1){
1102
    chprintf(chp, "%i steps \n", atoi(argv[0]));
1103
    steps = atoi(argv[0]);
1104
  }
1105
  else if(argc == 3){
1106
    distance = atoi(argv[0]);
1107
    angle = atoi(argv[0]);
1108
    time = atoi(argv[0]);
1109
  }else{
1110
    chprintf(chp, "Use: accel <steps>\n");
1111
    return;
1112
  }
1113
  global.distcontrol.setTargetPosition(distance, angle, time);
1114

  
1115
  for(int i=0; i<steps; i++){
1116
    int16_t Z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
1117
    int16_t X = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_X);
1118
    int16_t Y = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Y);
1119
    types::position pos = global.odometry.getPosition();
1120
    // chprintf((BaseSequentialStream*)&global.sercanmux1, "Axis X: %d, Y: %d, Z: %d\n", X, Y, Z);
1121
    chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d, Angle: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z, global.distcontrol.getCurrentTargetAngle());
1122
    // chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Z: %d\n", pos.f_x, pos.f_y, pos.f_z);
1123
    BaseThread::sleep(CAN::UPDATE_PERIOD);
1124
  }
1125

  
1126
  // global.motorcontrol.setGains(&global.motorConfigGains);
1127
}
965 1128

  
1129
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
1130
  types::position pos = global.odometry.getPosition();
1131
  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
  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);
1133
}
966 1134

  
967 1135
static const ShellCommand commands[] = {
968 1136
  {"shutdown", shellRequestShutdown},
......
989 1157
  {"motor_stop", shellRequestMotorStop},
990 1158
  {"motor_calibrate", shellRequestMotorCalibrate},
991 1159
  {"motor_getGains", shellRequestMotorGetGains},
1160
  {"motor_deactivate", freeGains},
1161
  {"motor_activate", setGains},
992 1162
  {"motor_resetGains", shellRequestMotorResetGains},
1163
  {"motorToggle", motorToggle},
993 1164
  {"dev_proxi_sensor_data", proxySensorData},
994 1165
  {"dev_ziegler2", zieglerMeth2},
995 1166
  // TODO: Stop user process from execution to finish/force calibration before anything starts
......
998 1169
  {"print_record", printMove},
999 1170
  {"setRecord", setRecord},
1000 1171
  {"followLine", followLine},
1172
  {"rotate", rotate},
1173
  {"followRotate", followAndRotate},
1174
  {"accel", getAccel},
1175
  {"getPos", getPosition},
1001 1176
  {NULL, NULL}
1002 1177
};
1003 1178

  

Also available in: Unified diff