Revision 9c46b728 devices/DiWheelDrive/main.cpp
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 1160 | 1160 |
|
| 1161 | 1161 |
} |
| 1162 | 1162 |
|
| 1163 |
void enableCharging(){
|
|
| 1164 |
global.ltc4412.enable(true); |
|
| 1165 |
} |
|
| 1163 | 1166 |
|
| 1164 |
void getAccel(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 1165 |
int steps = 0; |
|
| 1166 |
// global.motorcontrol.getGains(&global.motorConfigGains); |
|
| 1167 |
// global.motorcontrol.setGains(&global.stopGains); |
|
| 1168 |
int16_t time = 10000; |
|
| 1169 |
int32_t angle = 3141592; |
|
| 1170 |
int32_t distance = 0; |
|
| 1171 |
if (argc == 1){
|
|
| 1172 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
| 1173 |
steps = atoi(argv[0]); |
|
| 1174 |
} |
|
| 1175 |
else if(argc == 3){
|
|
| 1176 |
distance = atoi(argv[0]); |
|
| 1177 |
angle = atoi(argv[0]); |
|
| 1178 |
time = atoi(argv[0]); |
|
| 1179 |
}else{
|
|
| 1180 |
chprintf(chp, "Use: accel <steps>\n"); |
|
| 1181 |
return; |
|
| 1182 |
} |
|
| 1183 |
global.distcontrol.setTargetPosition(distance, angle, time); |
|
| 1167 |
void disableCharging(){
|
|
| 1168 |
global.ltc4412.enable(false); |
|
| 1169 |
} |
|
| 1184 | 1170 |
|
| 1185 |
for(int i=0; i<steps; i++){
|
|
| 1186 |
int16_t Z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
|
| 1187 |
int16_t X = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_X); |
|
| 1188 |
int16_t Y = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Y); |
|
| 1189 |
types::position pos = global.odometry.getPosition(); |
|
| 1190 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Axis X: %d, Y: %d, Z: %d\n", X, Y, Z); |
|
| 1191 |
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()); |
|
| 1192 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Z: %d\n", pos.f_x, pos.f_y, pos.f_z); |
|
| 1193 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1194 |
} |
|
| 1195 | 1171 |
|
| 1196 |
// global.motorcontrol.setGains(&global.motorConfigGains); |
|
| 1172 |
void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 1173 |
if(argc == 1){
|
|
| 1174 |
|
|
| 1175 |
global.lfStrategy = atoi(argv[0]); |
|
| 1176 |
} |
|
| 1197 | 1177 |
} |
| 1198 | 1178 |
|
| 1199 | 1179 |
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
|
| ... | ... | |
| 1210 | 1190 |
} |
| 1211 | 1191 |
} |
| 1212 | 1192 |
|
| 1193 |
// TODO: Not wokring, either loading station has no power or logic not working |
|
| 1194 |
void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 1195 |
int steps = 2000; |
|
| 1196 |
int led = 0; |
|
| 1197 |
enableCharging(); |
|
| 1198 |
for (int i=0; i<steps;i++){
|
|
| 1199 |
chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); |
|
| 1200 |
if(global.ltc4412.isPluggedIn()){
|
|
| 1201 |
// enableCharging(); |
|
| 1202 |
for(led=0; led<8; led++){
|
|
| 1203 |
global.robot.setLightColor(led, Color(Color::GREEN)); |
|
| 1204 |
} |
|
| 1205 |
}else{
|
|
| 1206 |
// disableCharging(); |
|
| 1207 |
for(led=0; led<8; led++){
|
|
| 1208 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 1209 |
} |
|
| 1210 |
} |
|
| 1211 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1212 |
} |
|
| 1213 |
disableCharging(); |
|
| 1214 |
for(led=0; led<8; led++){
|
|
| 1215 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 1216 |
} |
|
| 1217 |
} |
|
| 1218 |
|
|
| 1219 |
|
|
| 1213 | 1220 |
static const ShellCommand commands[] = {
|
| 1214 | 1221 |
{"shutdown", shellRequestShutdown},
|
| 1215 | 1222 |
{"wakeup", shellRequestWakeup},
|
| ... | ... | |
| 1249 | 1256 |
{"followLine", followLine},
|
| 1250 | 1257 |
{"rotate", rotate},
|
| 1251 | 1258 |
{"followRotate", followAndRotate},
|
| 1252 |
{"accel", getAccel},
|
|
| 1253 | 1259 |
{"getPos", getPosition},
|
| 1260 |
{"checkPower", checkPower},
|
|
| 1261 |
{"setStrategy", setGlobalStrategy},
|
|
| 1254 | 1262 |
{NULL, NULL}
|
| 1255 | 1263 |
}; |
| 1256 | 1264 |
|
Also available in: Unified diff