Esempio n. 1
0
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);
	}
}