Revision bfffb0bd

View differences:

devices/DiWheelDrive/global.hpp
187 187
  int accumHist = 0;
188 188
  int oldError = 0;
189 189

  
190
// Struct for saving motor gains
191

  
192
int resetProtect = 1;
193
motorGains motorConfigGains;
194
motorGains stopGains;
195

  
196
types::position startPos;
197
types::position endPos;
198

  
190 199
// Debugging stuff --------------
191 200
  int forwardSpeed = 10;
192
  int enableRecord = 1;
201
  int enableRecord = 0;
202

  
203
  
193 204

  
194 205
  // Buffer for sensor values
195 206
  struct sensorRecord
devices/DiWheelDrive/linefollow2.cpp
24 24
    int error = 0;
25 25
    switch (this->strategy)
26 26
    {
27
    case LineFollowStrategy::EDGE_RIGHT:
27
    case LineFollowStrategy::EDGE_RIGHT: case LineFollowStrategy::DOCKING:
28 28
        error = -(FL -targetL + FR - targetR);
29 29
        break;
30 30
    case LineFollowStrategy::EDGE_LEFT:
......
57 57
}
58 58

  
59 59
int LineFollow::followLine(int (&rpmSpeed)[2]){
60

  
60
    int correctionSpeed = 0;
61 61
    switch (this->strategy)
62 62
    {
63 63
    case LineFollowStrategy::FUZZY:
64
        for (int i = 0; i < 4; i++) {
65
                vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
66
                vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
67
            }
64
      for (int i = 0; i < 4; i++) {
65
          vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
66
          vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
67
      }
68
      lineFollowing(vcnl4020Proximity, rpmSpeed);
69
      break;
70

  
71
    case LineFollowStrategy::DOCKING:
72
      correctionSpeed = -getPidCorrectionSpeed();
73
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;
74

  
75
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;
76
      
77
      break;
68 78

  
69
        lineFollowing(vcnl4020Proximity, rpmSpeed);
70
        break;
71
    
72 79
    default:
73
        int correctionSpeed = getPidCorrectionSpeed();
74
        // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
80
      correctionSpeed = getPidCorrectionSpeed();
81
      // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
75 82

  
76
        rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
83
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
77 84

  
78
        rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
79
        return whiteFlag;
80
        break;
85
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
86
      break;
81 87
    }
88
    return whiteFlag;
82 89
}
83 90

  
84 91

  
devices/DiWheelDrive/linefollow2.hpp
9 9
  enum class LineFollowStrategy{
10 10
  EDGE_LEFT,
11 11
  EDGE_RIGHT,
12
  DOCKING,
12 13
  MIDDLE,
13 14
  FUZZY
14 15
};
......
73 74

  
74 75
    char whiteFlag = 0;
75 76
    LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
76
    float Kp = 0.003;
77
    float Kp = 0.002;
77 78
    float Ki = 0;
78 79
    float Kd = 0;
79 80
    int accumHist = 0;
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

  
devices/DiWheelDrive/userthread.cpp
56 56
const int sizeOfPolicy = sizeof(policy) / sizeof(states);
57 57
int policyCounter = 0; // Do not change this, it points to the beginning of the policy
58 58

  
59
// Different speed settings (all values in "rounds per minute")
60
// const int rpmTurnLeft[2] = {-10, 10};
61
// const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
62
// const int rpmHalt[2] = {0, 0};
63

  
64
// Definition of the fuzzyfication function
65
//  | Membership
66
// 1|_B__   G    __W__
67
//  |    \  /\  /
68
//  |     \/  \/
69
//  |_____/\__/\______ Sensor values
70
// SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
71
// All values are "raw sensor values"
72
/* Use these values for white ground surface (e.g. paper) */
73

  
74
// const int blackStartFalling = 0x1000; // Where the black curve starts falling
75
// const int blackOff = 0x1800; // Where no more black is detected
76
// const int whiteStartRising = 0x2800; // Where the white curve starts rising
77
// const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
78
// const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
79
// const int greyStartRising = blackStartFalling; // Where grey starts rising
80
// const int greyOff = whiteOn; // Where grey is completely off again
81

  
82
/* Use these values for gray ground surfaces */
83
/*
84
const int blackStartFalling = 0x1000; // Where the black curve starts falling
85
const int blackOff = 0x2800; // Where no more black is detected
86
const int whiteStartRising = 0x4000; // Where the white curve starts rising
87
const int whiteOn = 0x5000; // Where the white curve starts rising
88
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
89
const int greyStartRising = blackStartFalling; // Where grey starts rising
90
const int greyOff = whiteOn; // Where grey is completely off again
91
*/
59

  
92 60

  
93 61
int vcnl4020AmbientLight[4] = {0};
94 62
int vcnl4020Proximity[4] = {0};
......
206 174
                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
207 175
                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
208 176
            }
177

  
178

  
209 179
			// lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);
210 180
            // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
211 181
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
......
216 186
	//   lineFollownew
217 187
	//else
218 188
            // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
189
			lf.followLine(rpmFuzzyCtrl);
219 190
            setRpmSpeed(rpmFuzzyCtrl);
220 191
        }
221 192

  

Also available in: Unified diff