int main(int argc, char **argv) {
while (true) {
out.print("Enter some text: ");
char buf[20];
out.getline(buf, sizeof(buf));
out.printf("You typed: %s\n", buf);
Task::sleep(1000);
}
}
static void docalibrate(const char *type, PID::Config *conf) {
out.printf("Old %s: %f %f %f\n", type, conf->p, conf->i, conf->d);
out.printf("New %s PID values:\n", type);
static char buf[100];
out.getline(buf, sizeof(buf));
sscanf(buf, "%f %f %f", &conf->p, &conf->i, &conf->d);
out.printf("Got: %f %f %f\n", conf->p, conf->i, conf->d);
}
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);
}
}
