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);
}
}
/* ----------------------------------------------------------------------------
* メインループ
* ---------------------------------------------------------------------------- */
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();
}
