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