Beispiel #1
0
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);
	}
}