Revision 7f4e10f7

View differences:

devices/DiWheelDrive/main.cpp
17 17
#include <chprintf.h>
18 18
#include <shell.h>
19 19

  
20
#include "linefollow2.hpp" 
20
#include "linefollow.hpp" 
21 21

  
22 22
using namespace chibios_rt;
23 23

  
......
768 768

  
769 769

  
770 770

  
771
void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
771
void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
772 772
  // uint16_t vcnl4020AmbientLight[4];
773 773
  uint16_t vcnl4020Proximity[4];
774 774
  uint16_t rounds = 1;
775
  // uint16_t proxyL = global.threshProxyL;
776
  // uint16_t proxyR = global.threshProxyR;
777
  // uint16_t maxDelta = 0;
778
  
779
  // int sensorL = 0;
780 775
  // int sensorR = 0;
781 776
  if (argc == 1){
782 777
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
783 778
    rounds = atoi(argv[0]);
784 779
    
780
  } else {
781
    chprintf(chp, "Usage: dev_proxi_sensor_data <rounds> \n");
785 782
  }
786 783
  global.motorcontrol.getGains(&global.motorConfigGains);
787 784
  global.motorcontrol.setGains(&global.stopGains);
......
794 791
    
795 792
    uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
796 793
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
797
    // // Update proximity thresh
798
    // if (delta > maxDelta) {
799
    //   maxDelta = delta;
800
    //   proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
801
    //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
802
    // }
803

  
804
    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
805
    //   delta *= -1;
806
    // }
807 794

  
808 795
    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n", 
809 796
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
......
837 824
  // int proxyR = global.threshProxyR;
838 825
  int maxDelta = 0;
839 826
  float KCrit = 0.0f;
827
  
840 828
  global.sensSamples = 0;
841 829
  global.maxDist.error = 0;
842 830
  LineFollow lf(&global);
......
850 838
  } else if (argc == 3){
851 839
    chprintf(chp, "KCrti %f\n", atof(argv[0]));
852 840
    chprintf(chp, "Steps %i\n", atoi(argv[1]));
853
    KCrit = atof(argv[0]);
854
    steps = atoi(argv[1]);
855
    global.forwardSpeed = atoi(argv[2]);
841
    global.K_p = atof(argv[0]);
842
    global.K_d = atof(argv[1]);
843
    steps = atoi(argv[2]);
844
    global.forwardSpeed = 15;
856 845
    
857 846
  }else{
858
    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps> (<speed>)");
847
    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <kd> <steps> (<speed>)");
859 848
    return;
860 849
  }
861
  global.K_p = KCrit;
862 850
  for(led=0; led<8; led++){
863 851
        global.robot.setLightColor(led, Color(Color::BLACK));
864 852
  }
......
871 859
  for(int s=0; s < steps; s++){
872 860
    
873 861
    checkWhite = lf.followLine(rpmSpeed);
862
    checkWhite = 0;
874 863
    // chprintf(chp,"S:%d,",s);
875 864
    // if(global.threshWhite)
876 865
    // if(s < it_switch){
......
986 975
}
987 976

  
988 977

  
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 978
void setSpeed(int (&rpmSpeed)[2]){
1025 979
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
1026 980
}
1027 981

  
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

  
1099
void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){
1100
  int steps = 10000;
1101
  int speed = 0;
1102
  int strategy = 0;
1103
  int led = 0;
1104
  int checkWhite = 0;
1105
  int rpmSpeed[2] = {0};
1106
  int proxyWheelThresh = 20000;
1107
  LineFollow lf(&global);
1108
  if (argc == 1){
1109
      chprintf(chp, "%i steps \n", atoi(argv[0]));
1110
      speed = atoi(argv[0]);
1111
    }else if (argc == 2){
1112
      speed = atoi(argv[0]);
1113
      steps = atoi(argv[1]);
1114
    }else{
1115
      chprintf(chp, "Use: rotate <speed> <steps>\n");
1116
      return;
1117
    }
1118
    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
1119
    BaseThread::sleep(MS2ST(5000));
1120
    global.forwardSpeed = speed;
1121
    
1122
    setSpeed(rpmSpeed);
1123

  
1124

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

  
1127
      int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1128
      int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
1129
      if ((WL+WR) < proxyWheelThresh){
1130
        global.motorcontrol.setTargetRPM(0,0);
1131
        chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n");
1132
        if(lf.getStrategy() == LineFollowStrategy::DOCKING){
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
          
1142
        }else{
1143
          // global.motorcontrol.setTargetRPM(0,0);
1144
          BaseThread::sleep(1000);
1145
          // 180° Rotation 
1146
          global.distcontrol.setTargetPosition(0, 3141592, 10000);
1147
          // BaseThread::sleep(8000);
1148
          checkForMotion();
1149
          correctPosition(lf, 250);
1150
          // break;
1151
          lf.setStrategy(LineFollowStrategy::DOCKING);
1152
          
1153
        }
1154
      }
1155
      checkWhite = lf.followLine(rpmSpeed);
1156
      setSpeed(rpmSpeed);
1157
      BaseThread::sleep(CAN::UPDATE_PERIOD);
1158
  }
1159

  
1160
  
1161
}
1162 982

  
1163 983
void enableCharging(){
1164 984
  global.ltc4412.enable(true);
......
1199 1019
void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){
1200 1020
  int steps = 2000;
1201 1021
  int led = 0;
1202
  enableCharging();
1203
  for (int i=0; i<steps;i++){
1204
    chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); 
1205
    if(global.ltc4412.isPluggedIn()){
1206
      // enableCharging();
1207
      for(led=0; led<8; led++){
1208
        global.robot.setLightColor(led, Color(Color::GREEN));
1209
      }
1022
  // enableCharging();
1023
  // for (int i=0; i<steps;i++){
1024
  //   chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); 
1025
  //   if(global.ltc4412.isPluggedIn()){
1026
  //     // enableCharging();
1027
  //     for(led=0; led<8; led++){
1028
  //       global.robot.setLightColor(led, Color(Color::GREEN));
1029
  //     }
1030
  //   }else{
1031
  //     // disableCharging();
1032
  //     for(led=0; led<8; led++){
1033
  //       global.robot.setLightColor(led, Color(Color::RED));
1034
  //     }
1035
  //   }
1036
  //   BaseThread::sleep(CAN::UPDATE_PERIOD);
1037
  // }
1038
  // disableCharging();
1039
  // for(led=0; led<8; led++){
1040
  //   global.robot.setLightColor(led, Color(Color::BLACK));
1041
  // }
1042
  // 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]);
1043
  chprintf(chp, "Power over Pins: %s Pins Enabled: %s, Power: %d\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y" : "n", global.robot.getPowerStatus().state_of_charge);
1044
}
1045

  
1046

  
1047
void proxyRing(BaseSequentialStream *chp, int argc, char *argv[]){
1048
   int steps = 10000;
1049
   int i;
1050

  
1051
  uint16_t prox[8];
1052
  uint32_t prox_sum = 0;
1053

  
1054
  if (argc == 1){
1055
      chprintf(chp, "%i steps \n", atoi(argv[0]));
1056
      steps = atoi(argv[0]);
1210 1057
    }else{
1211
      // disableCharging();
1212
      for(led=0; led<8; led++){
1213
        global.robot.setLightColor(led, Color(Color::RED));
1214
      }
1058
      chprintf(chp, "Usage: proxyRing <steps> \n");
1059
    }
1060
  for (int j=0; j<steps; j++){
1061
    prox_sum = 0;
1062
    for(i=0; i<8;i++){
1063
      prox[i] = global.robot.getProximityRingValue(i);
1064
      prox_sum += prox[i];
1215 1065
    }
1066
    // uint16_t notouch = 100;
1067
    // uint16_t toucht = 20031;
1068
    // sign = 
1069
    // i = 0;
1070
    chprintf(chp, "0:%i 1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i prox_sum:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], prox_sum);
1216 1071
    BaseThread::sleep(CAN::UPDATE_PERIOD);
1217 1072
  }
1218
  disableCharging();
1219
  for(led=0; led<8; led++){
1220
    global.robot.setLightColor(led, Color(Color::BLACK));
1221
  }
1222 1073
}
1223 1074

  
1224 1075

  
......
1251 1102
  {"motor_activate", setGains},
1252 1103
  {"motor_resetGains", shellRequestMotorResetGains},
1253 1104
  {"motorToggle", motorToggle},
1254
  {"dev_proxi_sensor_data", proxySensorData},
1105
  {"dev_proxi_sensor_data", getProxySensorData},
1255 1106
  {"dev_ziegler2", zieglerMeth2},
1256 1107
  // TODO: Stop user process from execution to finish/force calibration before anything starts
1257 1108
  {"calibrate_line", calibrateLineSensores}, 
......
1259 1110
  {"print_record", printMove},
1260 1111
  {"setRecord", setRecord},
1261 1112
  {"followLine", followLine},
1262
  {"rotate", rotate},
1263
  {"followRotate", followAndRotate},
1264 1113
  {"getPos", getPosition},
1265 1114
  {"checkPower", checkPower},
1266 1115
  {"setStrategy", setGlobalStrategy},
1116
  {"proxyRing", proxyRing},
1267 1117
  {NULL, NULL}
1268 1118
};
1269 1119

  

Also available in: Unified diff