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);
}
