Revision bfffb0bd
| 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