int main(int argc, char **argv) {
bool prevsynced=true;
while (true) {
Task::sleep(5);
if (vex.getSynced()) {
VexRC::Channels chans = vex.getChannels();
out.printf("%d %d %d %d ", chans.analogs[0], chans.analogs[1], chans.analogs[2], chans.analogs[3]);
out.printf("%s %s\n", digitalToString(chans.left), digitalToString(chans.right));
prevsynced = true;
} else {
if (prevsynced) {
buzzer.buzz(100);
out.print("Not synced\n");
}
prevsynced = false;
}
}
}
int main(int argc, char **argv) {
imu.start();
out.printf("Battery: %f cell\n", batmon.getCellVoltage());
out.print("Waiting for remote signal\n");
while (!vex.getSynced()) { Task::sleep(100); }
out.print("Arming\n");
motors.arm();
float heading = imu.getYaw();
buzzer.buzz(300);
while (true) {
control.start();
Logger logger(eeprom, 0);
bool logging = false;
while (true) {
bool up = false;
if (!vex.getSynced()) {
control.stop();
motors.off();
while (!vex.getSynced()) { Task::sleep(100); }
control.start();
sensors.centerGyros();
heading = imu.getYaw();
buzzer.buzz(300);
}
VexRC::Channels chans = vex.getChannels();
if (chans.right != VexRC::NONE)
break;
if (chans.left == VexRC::UP)
logging = true;
else if (chans.left == VexRC::DOWN)
logging = false;
float throttle = chans.analogs[1] / 50.0;
if (throttle < 0)
throttle = 0;
float rollsetpoint = (-chans.analogs[3] / 50.0) * 0.3;
float pitchsetpoint = (-chans.analogs[2] / 50.0) * 0.3;
heading += (chans.analogs[0] / 50.0) * (M_PI / 4) * .005;
control.setControlPoints(throttle, rollsetpoint, pitchsetpoint, heading);
if (!up) {
if (throttle > 0.4)
up = true;
else
control.clearIntegrals();
}
if (logging) {
LogEntry entry = { sensors.getReadings(), imu.getState(), throttle };
logger.write((uint8_t *)&entry, sizeof(entry));
}
IMU::State state = imu.getState();
IMU::State velstate = imu.getVelocityState();
out.printf("%f %f\n", control.getRollCorrection(), control.getPitchCorrection());
Task::sleep(25);
}
control.stop();
motors.off();
out.print("Push enter\n");
while (out.getch() != '\r') { }
out.print("Press y to dump log");
if (out.getch() == 'y') {
out.print("\nLog dump:");
LogReader reader(eeprom, 0);
struct LogEntry entry;
while (reader.read((uint8_t *)&entry, sizeof(entry)) == sizeof(entry)) {
int i;
for (i=0;i<6;i++) {
out.printf("%.3f ", entry.analogs.array[i]);
}
out.printf("%.3f %.3f %.3f ", entry.state.roll, entry.state.pitch, entry.state.yaw);
out.printf("%.3f\n", entry.throttle);
}
}
docalibrate("Roll", &controlconfig.roll_config);
docalibrate("Pitch", &controlconfig.pitch_config);
docalibrate("Yaw", &controlconfig.yaw_config);
out.printf("Current stick values: %f %f\n", controlconfig.rollpitch_stickfactor, controlconfig.yaw_stickfactor);
out.printf("Stick: ");
static char buf[50];
out.getline(buf, sizeof(buf));
sscanf(buf, "%f %f", &controlconfig.rollpitch_stickfactor, &controlconfig.yaw_stickfactor);
}
}