Ejemplo n.º 1
0
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);
	}
}
Ejemplo n.º 2
0
/* ----------------------------------------------------------------------------
 * メインループ
 * ---------------------------------------------------------------------------- */
void loop() {
	int i;
	int j;
	
	// 1秒毎の処理
	if(interruptCount > 999) {
		// 時計モード
		if(userDataTimerCount == 0) {
			rtc.get();
			// 年月日表示か時刻表示かの判定
			if(displayDateIntervalCount <= displayDateIntervalValue) {
				// 時分秒表示
				// カンマをクリア
				for(int i = 0; i < 8; i++) {
					IN12Com[i] = 0;
				}
				// 6桁フォーマット 999999
				if(dispTimeFormat == 0) {
					IN12Num[0] = 0x0F;
					if(hour12_24format == 1) {
						IN12Num[1] = rtc.hourHigh;
						IN12Num[2] = rtc.hourLow;
					} else {
						IN12Num[1] = rtc.hourHigh12;
						IN12Num[2] = rtc.hourLow12;
					}
					IN12Num[3] = rtc.minutesHigh;
					IN12Num[4] = rtc.minutesLow;
					IN12Num[5] = rtc.secondsHigh;
					IN12Num[6] = rtc.secondsLow;
					IN12Num[7] = 0x0F;
				}
				// 8桁フォーマット 99, 99, 99
				if(dispTimeFormat == 1) {
					if(hour12_24format == 1) {
						IN12Num[0] = rtc.hourHigh;
						IN12Num[1] = rtc.hourLow;
					} else {
						IN12Num[0] = rtc.hourHigh12;
						IN12Num[1] = rtc.hourLow12;
					}
					IN12Num[2] = 0x0F;
					IN12Num[3] = rtc.minutesHigh;
					IN12Num[4] = rtc.minutesLow;
					IN12Num[5] = 0x0F;
					IN12Num[6] = rtc.secondsHigh;
					IN12Num[7] = rtc.secondsLow;
					IN12Com[2] = 1;
					IN12Com[5] = 1;
				}
			} else {
				// 年月日表示
				// カンマクリア
				for(int i = 0; i < 8; i++) {
					IN12Com[i] = 0;
				}
				IN12Num[0] = 2;
				IN12Num[1] = 0;
				IN12Num[2] = rtc.yearHigh;
				IN12Num[3] = rtc.yearLow;
				IN12Num[4] = rtc.monthHigh;
				IN12Num[5] = rtc.monthLow;
				IN12Num[6] = rtc.dayHigh;
				IN12Num[7] = rtc.dayLow;
				if(displayDateIntervalCount >= ( displayDateIntervalValue + displayDateSecondsValue)) {
					displayDateIntervalCount = 0;
				}
			}
			displayDateIntervalCount++;
		}
		// ユーザーデータ表示中
		if(userDataTimerCount > 0) {
			userDataTimerCount--;
		}
		// LED 点滅
		if(led == false) {
			led = true;
			PORTC |= (1<<LED);
		} else {
			led = false;
			PORTC &= ~(1<<LED);
		}
		interruptCount = 0;
	}
	
	// シリアル1 受信処理
	while(Serial1.available()) {
		char ch = Serial1.getch();
		serial0String[serial0StringIdx] = ch;
		Serial1.putch(ch);
		if(serial0String[serial0StringIdx] == '\r') {
			Serial1.putch('\n');
			serial0String[serial0StringIdx] = 0;
			serial0StringComplete = true;
		}
		serial0StringIdx++;
		if(serial0StringIdx >= SERIALBUF_LEN) {
			serial0StringIdx = SERIALBUF_LEN - 1;
		}
		// CR コードだけの入力は無視
		if(serial0StringComplete == true && serial0StringIdx == 1) {
			serial0StringIdx = 0;
			serial0StringComplete = false;
		}
	}
	
	// 受信データ処理
	if(serial0StringComplete == true) {
		// ニキシー表示データチェック
		int isUserData = true;
		for(i = 0; i < (serial0StringIdx - 1); i++) {
			if(serial0String[i] == ' ') continue;
			if(serial0String[i] == '.') continue;
			if((serial0String[i] >= '0') && (serial0String[i] <= '9')) continue;
			isUserData = false;
		}
		// ユーザーデータを表示
		if(isUserData == true) {
			for(i = 0; i < 8; i++) {
				IN12Num[i] = 0x0F;
				IN12Com[i] = 0;
			}
			j = 7;
			for(i = (serial0StringIdx - 1); i >= 0; i--) {
				if(serial0String[i] >= '0' && serial0String[i] <= '9') {
					if(j >= 0) {
						IN12Num[j--] = serial0String[i] - 0x30;
					}
				} else if(serial0String[i] == ' ') {
					if(j >= 0) {
						IN12Num[j--] = 0x0F;
					}
				}
			}
			j = 8;
			int previusCharValid = false;
			for(i = (serial0StringIdx - 1); i >= 0; i--) {
				if((serial0String[i] >= '0' && serial0String[i] <= '9') || serial0String[i] == ' ') {
					previusCharValid = true;
					if(j > 0) {
						j--;
					}
					continue;
				}
				if(serial0String[i] == '.' && previusCharValid == true) {
					IN12Com[j] = 1;
					previusCharValid = false;
				}
			}
			userDataTimerCount = userDataTimerValue;
		}
		if(isUserData == false) {
			// コマンド処理
			if(strncmp(serial0String, "help", 4) == 0) {
				Serial1.putstr("set time YYMMDD hhmmss\r\n");
				Serial1.putstr("set timeformat [0 or 1]\r\n");
				Serial1.putstr("set dateinterval 99999\r\n");
				Serial1.putstr("set datesec 99999\r\n");
				Serial1.putstr("set udatasec 99999\r\n");
				Serial1.putstr("set bright daytime [1 to 9]\r\n");
				Serial1.putstr("set bright night [1 to 9]\r\n");
				Serial1.putstr("set hour daytime [0 to 23]\r\n");
				Serial1.putstr("set hour night [0 to 23]\r\n");
				Serial1.putstr("set 12/24 format [0 or 1]\r\n");
				Serial1.putstr("cathod -> Cathod poisoning\r\n");
				Serial1.putstr("sekai  -> STEINS;GATE like effect\r\n");
				Serial1.putstr("save   -> write EEPROM\r\n");
				Serial1.putstr("show   -> show all setting\r\n");
				Serial1.putstr("time   -> change to clock mode\r\n");
				Serial1.putstr("\r\n");
				Serial1.putstr("99999: numeric number 0 to 65535\r\n");
				Serial1.putstr("---\r\n");
			}
			if(strncmp(serial0String, "set timeformat ", 15) == 0) {
				unsigned int i = atoi(&serial0String[15]);
				if(i == 0 || i == 1) {
					dispTimeFormat = i;
					sprintf(msg, "value = %d\r\n", dispTimeFormat);
					Serial1.putstr(msg);
				}
			}
			if(strncmp(serial0String, "set time ", 9) == 0) {
				unsigned char year = ((serial0String[9] & 0x0F)<<4);
				year |= serial0String[10] & 0x0F;
				unsigned char month = ((serial0String[11] & 0x0F)<<4);
				month |= serial0String[12] & 0x0F;
				unsigned char days = ((serial0String[13] & 0x0F)<<4);
				days |= serial0String[14] & 0x0F;
				unsigned char hour = ((serial0String[16] & 0x0F)<<4);
				hour |= serial0String[17] & 0x0F;
				unsigned char minutes = ((serial0String[18] & 0x0F)<<4);
				minutes |= serial0String[19] & 0x0F;
				unsigned char seconds = ((serial0String[20] & 0x0F)<<4);
				seconds |= serial0String[21] & 0x0F;
				rtc.set(year, month, 1, days, hour, minutes, seconds);
			}
			if(strncmp(serial0String, "set dateinterval ", 17) == 0) {
				displayDateIntervalValue = atoi(&serial0String[17]);
				sprintf(msg, "value = %d\r\n", displayDateIntervalValue);
				Serial1.putstr(msg);
			}
			if(strncmp(serial0String, "set datesec ", 12) == 0) {
				displayDateSecondsValue = atoi(&serial0String[12]);
				sprintf(msg, "value = %d\r\n", displayDateSecondsValue);
				Serial1.putstr(msg);
			}
			if(strncmp(serial0String, "set udatasec ", 13) == 0) {
				userDataTimerValue = atoi(&serial0String[13]);
				sprintf(msg, "value = %d\r\n", userDataTimerValue);
				Serial1.putstr(msg);
			}
			if(strncmp(serial0String, "cathod", 6) == 0) {
				cathodePoisoning();
				Serial1.putstr("Done.");
			}
			if(strncmp(serial0String, "sekai", 5) == 0) {
				steins();
				//serialPrintln("Done.");
			}
			if(strncmp(serial0String, "set bright daytime ", 19) == 0) {
				unsigned int i = atoi(&serial0String[19]);
				if(i > 0 && i < 10) {
					IN12BrightDaytime = i;
					sprintf(msg, "value = %d\r\n", IN12BrightDaytime);
					Serial1.putstr(msg);
				}
			}
			if(strncmp(serial0String, "set bright night ", 17) == 0) {
				unsigned int i = atoi(&serial0String[17]);
				if(i > 0 && i < 10) {
					IN12BrightNight = i;
					sprintf(msg, "value = %d\r\n",IN12BrightNight);
					Serial1.putstr(msg);
				}
			}
			if(strncmp(serial0String, "set hour daytime ", 17) == 0) {
				unsigned int i = atoi(&serial0String[17]);
				if(i >= 0 && i < 24) {
					hourDaytime = i;
					sprintf(msg, "value = %d\r\n", hourDaytime);
					Serial1.putstr(msg);
				}
			}
			if(strncmp(serial0String, "set hour night ", 15) == 0) {
				unsigned int i = atoi(&serial0String[15]);
				if(i >= 0 && i < 24) {
					hourNight = i;
					sprintf(msg, "value = %d\r\n", hourNight);
					Serial1.putstr(msg);
				}
			}
			if(strncmp(serial0String, "set 12/24 format ", 17) == 0) {
				unsigned int i = atoi(&serial0String[17]);
				if(i == 0 || i == 1) {
					hour12_24format = i;
					sprintf(msg, "value = %d\r\n", hour12_24format);
					Serial1.putstr(msg);
				}
			}
			if(strncmp(serial0String, "save", 4) == 0) {
				// EEPROM 書き込み
				eeprom_busy_wait();
				eeprom_write_word(&E_displayDateIntervalValue, displayDateIntervalValue);
				eeprom_busy_wait();
				eeprom_write_word(&E_displayDateSecondsValue, displayDateSecondsValue);
				eeprom_busy_wait();
				eeprom_write_word(&E_userDataTimerValue, userDataTimerValue);
				eeprom_busy_wait();
				eeprom_write_byte(&E_dispTimeFormat, dispTimeFormat);
				eeprom_busy_wait();
				eeprom_write_byte(&E_IN12BrightDaytime, IN12BrightDaytime);
				eeprom_busy_wait();
				eeprom_write_byte(&E_IN12BrightNight, IN12BrightNight);
				eeprom_busy_wait();
				eeprom_write_byte(&E_hourDaytime, hourDaytime);
				eeprom_busy_wait();
				eeprom_write_byte(&E_hourNight, hourNight);
				eeprom_busy_wait();
				eeprom_write_byte(&E_hour12_24format, hour12_24format);
				Serial1.putstr("EEPROM write complete\r\n");
			}
			if(strncmp(serial0String, "time", 4) == 0) {
				userDataTimerCount = 0;
			}
			if(strncmp(serial0String, "show", 4) == 0) {
				sprintf(msg, "set timeformat %d\r\n",dispTimeFormat);
				Serial1.putstr(msg);
				sprintf(msg, "set dateinterval: %d\r\n",displayDateIntervalValue);
				Serial1.putstr(msg);
				sprintf(msg, "set datesec: %d\r\n", displayDateSecondsValue);
				Serial1.putstr(msg);
				sprintf(msg, "set udatasec: %d\r\n", userDataTimerValue);
				Serial1.putstr(msg);
				sprintf(msg, "set bright daytime: %d\r\n", IN12BrightDaytime);
				Serial1.putstr(msg);
				sprintf(msg, "set bright night: %d\r\n", IN12BrightNight);
				Serial1.putstr(msg);
				sprintf(msg, "set hour daytime: %d\r\n", hourDaytime);
				Serial1.putstr(msg);
				sprintf(msg, "set hour night: %d\r\n", hourNight);
				Serial1.putstr(msg);
				sprintf(msg, "set 12/24 format: %d\r\n", hour12_24format);
				Serial1.putstr(msg);
				Serial1.putstr("Release 2013/11/03 Ver 1.0\r\n");
				Serial1.putstr("---\r\n");
			}
		}
		serial0StringIdx = 0;
		serial0StringComplete = false;
	}
	wdt_reset();
}