int main(int argc, char *argv[]) { const Option opt(argc, argv); cybozu::Socket server; server.bind(opt.recvPort); std::thread cmdWorker(cmdThread, opt); std::vector<std::unique_ptr<Repeater>> worker; try { for (size_t i = 0; i < opt.threadNum; i++) { worker.emplace_back(new Repeater(opt)); } while (!g_quit) { RETRY: while (!server.queryAccept()) { #if 0 if (opt.verbose) { printf("worker state "); for (size_t i = 0; i < opt.threadNum; i++) { printf("%d ", worker[i]->getState()); } printf("\n"); } #endif } if (g_quit) break; cybozu::SocketAddr addr; cybozu::Socket client; server.accept(client, &addr); if (opt.verbose) fprintf(stderr, "accept addr %s\n", addr.toStr().c_str()); while (!g_quit) { for (size_t i = 0; i < opt.threadNum; i++) { // if (opt.verbose) fprintf(stderr, "worker[%d] state=%d", (int)i, worker[i]->getState()); if (worker[i]->tryAndRun(client)) { if (opt.verbose) fprintf(stderr, "start %d repeater", (int)i); goto RETRY; } } waitMsec(100); } waitMsec(100); } } catch (std::exception& e) { fprintf(stderr, "ERR %s\n", e.what()); } g_quit = true; for (std::unique_ptr<Repeater>& p : worker) { p->join(); } cmdWorker.join(); if (opt.verbose) puts("main end"); }
void adjustDrive(int total_ticks, int power){ int ticks = 0; int left_power = power; int right_power = power; int diff = 0; SensorValue[leftDriveEncoder] = 0; SensorValue[rightDriveEncoder] = 0; while(abs(ticks) < total_ticks){ power_drive(left_power, right_power); diff = SensorValue[leftDriveEncoder] - SensorValue[rightDriveEncoder]; right_power += diff / 4; if(right_power > 100){ right_power = 100; } SensorValue[leftDriveEncoder] = 0; SensorValue[rightDriveEncoder] = 0; waitMsec(10); ticks += SensorValue[leftDriveEncoder]; } power_drive(0,0); }
int start_kernel(int argc, char *argv[]) { int i; int rc; int reg[16]; int cpsr; const int timer_irq = 49; cpsr = get_cpsr(); cpsr &= ~(1<<6 | 1<<7); /* enable irq & fiq */ cpsr = set_cpsr(cpsr); printk("starting kernel...\n"); printk("starting timer1...\n"); avic_init_irq((void *)0x80000000, 128); mxs_enable_fiq_functionality(1); icoll_unmask_irq(timer_irq); start_timer(0, 0, 0, 0); waitMsec(1000); printk("CPSR: [%08X]\n", getcpsr()); getr(reg); for(i=0; i<16; i++){ printk("register [%d]: [%X]\n", i, reg[i]); } printk("Hello World!\n"); while(1) { printk("E"); waitMsec(1000); } return 0; }
void init_gps(void) { PJOUT &= ~GPS_RESET; waitMsec(GPS_WAIT); PJOUT |= GPS_RESET; while(!(PJIN & GPS_PWRCHK)) { waitMsec(GPS_WAIT); PJOUT |= GPS_PWRCNTL; waitMsec(GPS_WAIT); PJOUT &= ~GPS_PWRCNTL; } for(int i = 0; i < sizeof GpsPacket; i++) GpsPacket[i] = 0x00; for(int i = 0; i < sizeof Lat; i++) { Lat[i] = 0x00; Lon[i] = 0x00; } }
int main(int argc, char *argv[]) try { const Option opt(argc, argv); cybozu::Socket server; server.bind(opt.recvPort); ThreadRunner cmdRunner; cmdRunner.set(std::thread(cmdThread, opt)); std::vector<std::unique_ptr<Repeater>> worker; for (size_t i = 0; i < opt.threadNum; i++) { worker.emplace_back(new Repeater(opt, (int)i)); } for (;;) { RETRY: while (!g_quit && !server.queryAccept()) { } if (g_quit) break; cybozu::SocketAddr addr; cybozu::Socket client; server.accept(client, &addr); setSocketTimeout(client, opt.socketTimeoutS); if (opt.verbose) cybozu::PutLog(cybozu::LogInfo, "accept addr %s", addr.toStr().c_str()); while (!g_quit) { for (size_t i = 0; i < opt.threadNum; i++) { if (worker[i]->tryAndRun(client)) { goto RETRY; } } waitMsec(100); } waitMsec(100); } if (opt.verbose) puts("main end"); } catch (std::exception& e) { cybozu::PutLog(cybozu::LogError, "error: %s", e.what()); return 1; }
bool tryAndRun(cybozu::Socket& client) { int expected = Sleep; if (!state_.compare_exchange_strong(expected, Ready)) return false; if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "tryAndRun:in"); try { s_[0].moveFrom(client); s_[1].connect(opt_.serverAddr, opt_.serverPort); state_ = Running; return true; } catch (std::exception& e) { cybozu::PutLog(cybozu::LogInfo, "tryAndRun::connect err %s", e.what()); s_[0].close(); s_[1].close(); state_ = Sleep; waitMsec(100); return true; } }
void main(void) { Init_Ports(); // Initialize Ports Init_Clocks(); // Initialize Clock System Init_Conditions(); Init_Interrupts(); //PJOUT |= LED1; // Turn LED 1 on to indicate boot TimeMsec = RESET_TIME; Init_Timers(); // Initialize Timers Init_LCD(); // Initialize LCD //Init_LEDs(); // Initialize LEDs // "0123456789abcdef" display_1 = " PROJECT 5 "; display_2 = " "; Display_Process(); P1OUT |= IR_LED; waitMsec(10); /* ---------- Begining of the "While" Operating System ------------- */ while(ALWAYS) // Can the Operating system run { if(TimeMsec % EVERY_50 == RESET_TIME) Display_Process(); //Refreshes screen every 50 'ticks' if(TimeMsec % EVERY_2 == RESET_TIME) { Switches_Process(); // Poll for switch state change every other 'tick' ADC_Process(); } Motors_Process(); Control_Process(); } }
void loop(int dir) try { if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "loop %d start", dir); assert(dir == 0 || dir == 1); cybozu::Socket &from = s_[dir]; cybozu::Socket &to = s_[1 - dir]; const bool needShutdown = dir == 1; const int intervalSec = 3; SMAverage sma(intervalSec); std::vector<char> buf(12 * 1024); while (!g_quit) { switch ((int)state_) { case Sleep: waitMsec(10); continue; case Ready: waitMsec(1); continue; case Error: if (!to.isValid()) { if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "loop %d %d toSleep", dir, (int)state_); state_ = Sleep; } else { waitMsec(1); } break; case Running: if (!from.isValid()) { cybozu::PutLog(cybozu::LogInfo, "loop %d %d from is not valid", dir, (int)state_); state_ = Error; continue; } try { while (!from.queryAccept()) { } if (g_quit) break; const size_t readSize = from.readSome(buf.data(), buf.size()); if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "loop %d %d readSize %d", dir, (int)state_, (int)readSize); if (opt_.rateMbps > 0) { sma.append(readSize, getCurTimeSec()); while (sma.getBps(getCurTimeSec()) * 1e-6 > opt_.rateMbps) { waitMsec(10); } } if (readSize > 0) { if (!g_stop && to.isValid()) to.write(buf.data(), readSize); continue; } } catch (std::exception& e) { cybozu::PutLog(cybozu::LogInfo, "loop %d %d ERR %s", dir, (int)state_, e.what()); state_ = Error; from.close(); continue; } if (needShutdown) { if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo,"loop %d %d shutdown", dir, (int)state_); const bool dontThrow = true; to.shutdown(1, dontThrow); // write disallow } if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "loop %d %d close", dir, (int)state_); from.close(); if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "loop %d %d isValid %d", dir, (int)state_, from.isValid()); break; } } if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "loop %d end", dir); } catch (...) { ep_ = std::current_exception(); s_[0].close(); s_[1].close(); state_ = Sleep; }
void start_timer(unsigned r0, unsigned r1, unsigned r2, unsigned r3) { int val; int i; #define BIT(n) (1<<n) #define BITTST(val, n) ((val) & BIT(n)) #define TST(val, b) ((val) & (b)) hw_timer_rotary[0] = (void *)(0x80068000); /* have ROTCTRL */ hw_timer_rotary[1] = (void *)(0x80068050); hw_timer_rotary[2] = (void *)(0x80068080); hw_timer_rotary[3] = (void *)(0x800680C0); val = hw_timer_rotary[0]->HW_TIMROT_ROTCTRL[0]; printk("This SoC has:\n"); if(BITTST(val,25)) printk("timer 0\n"); if(BITTST(val,26)) printk("timer 1\n"); if(BITTST(val,27)) printk("timer 2\n"); if(BITTST(val,28)) printk("timer 3\n"); #define IRQ (1<<15) #define IRQ_EN (1<<14) #define MATCH_MODE (1<<11) #define POLARITY (1<<8) #define UPDATE (1<<7) #define RELOAD (1<<6) #define PRESCALE(n) ((n)<<4) #define DIV_BY_8 (0x3) #define SELECT(n) ((n)<<0) #define TICK_ALWAYS (0XF) #define SET 1 #define CLR 2 #define TOG 3 hw_timer_rotary[1]->HW_TIMROT_FIXED_COUNT[0] = 0x00011000; val = IRQ_EN | UPDATE | RELOAD | PRESCALE(TICK_ALWAYS) | SELECT(0xB); out_w(HW_TIMROT_TIMCTRL1, val); while(1){ static int random = 0; random++; printk("[%04d]wait timer1 irq\n", random); waitMsec(100); val = hw_timer_rotary[1]->HW_TIMROT_TIMCTRL[0]; printk("%016b\n", val); val = in_w(HW_TIMROT_RUNNING_COUNT1); printk("timer running @[0x%x]\n", val); val = in_w(HW_TIMROT_VERSION); printk("timer version @[%X]\n", val); if(random>300) break; } printk("Goodbye TIMER!\n"); }
void Control_Process(void) { IsLight = (ADC_LeftIR + ADC_RightIR)/2 < ADC_Thumb; if(!IsLight) { // "0123456789abcdef" display_2 = " Dark "; if(WasLight && (ProgramState == 1 || ProgramState == 3)) ProgramState++; } else { display_2 = " Light "; } if((ProgramState != 1 && ProgramState != 3) && MotorTimer <= 0) ProgramState++; if(OldState != ProgramState) switch(ProgramState) { case 1: allStop(); waitMsec(500); MotorTimer = 10000; LeftMotorPower = LPOWER; RightMotorPower = RPOWER; // "0123456789abcdef" display_1 = "Forward to Line "; break; case 2: allStop(); waitMsec(500); LeftMotorPower = -LPOWER; RightMotorPower = -RPOWER; MotorTimer = 1000; // "0123456789abcdef" display_1 = " Blind Reverse "; break; case 3: allStop(); MotorTimer = 10000; LeftMotorPower = -LPOWER; RightMotorPower = -RPOWER; // "0123456789abcdef" display_1 = "Reverse to Line "; break; case 4: allStop(); //waitMsec(250); MotorTimer = (10000 - MotorTimer + 4000)/2; waitMsec(500); LeftMotorPower = LPOWER-10; RightMotorPower = RPOWER; // "0123456789abcdef" display_1 = "Forward to Mid "; break; case 5: allStop(); waitMsec(500); MotorTimer = SPIN_TIME*3; LeftMotorPower = LPOWER; RightMotorPower = -RPOWER; // "0123456789abcdef" display_1 = " Spin CW 3x "; break; case 6: allStop(); waitMsec(500); MotorTimer = SPIN_TIME*2; LeftMotorPower = -LPOWER; RightMotorPower = RPOWER; // "0123456789abcdef" display_1 = " Spin CCW 2x "; break; default: allStop(); MotorTimer = 0; // "0123456789abcdef" display_1 = " Default "; break; } WasLight = IsLight; OldState = ProgramState; }
void loop(int dir) try { if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "[%d] loop %d start", id_, dir); assert(dir == 0 || dir == 1); cybozu::Socket &from = s_[dir]; cybozu::Socket &to = s_[1 - dir]; const int intervalSec = 3; SMAverage sma(intervalSec); std::vector<char> buf(1024); while (!g_quit) { switch ((int)state_) { case Sleep: waitMsec(10); continue; case Ready: waitMsec(1); continue; case Running2: case Running1: if (!from.isValid()) { waitMsec(1); continue; } try { while (!g_quit && !from.queryAccept()) { } if (g_quit) continue; size_t readSize = from.readSome(buf.data(), buf.size()); if (opt_.rateMbps > 0) { sma.append(readSize, cybozu::GetCurrentTimeSec()); while (double rate = sma.getBps(cybozu::GetCurrentTimeSec()) > opt_.rateMbps * 1e6) { if (opt_.verbose) cybozu::PutLog(cybozu::LogDebug, "[%d] loop %d %d rate %f", id_, dir, (int)state_, rate); waitMsec(1); } } if (readSize > 0) { if (!g_stop && to.isValid()) { if (opt_.delaySec) { waitMsec(opt_.delaySec * 1000); } to.write(buf.data(), readSize); } } else { to.shutdown(1, dontThrow); // write disallow from.close(); state_--; } } catch (std::exception& e) { cybozu::PutLog(cybozu::LogInfo, "[%d] loop %d %d ERR %s", id_, dir, (int)state_, e.what()); from.close(dontThrow); state_--; } break; case Running0: { int expected = Running0; state_.compare_exchange_strong(expected, Sleep); } break; } } if (opt_.verbose) cybozu::PutLog(cybozu::LogInfo, "[%d] loop %d end", id_, dir); } catch (std::exception& e) { cybozu::PutLog(cybozu::LogError, "[%d] caught an error and terminate: %s", id_, e.what()); exit(1); }