Shinichiro Nakamura
/
Drive2ChoroQ
This is a demonstration of two Choro Q Hybrid cars.
main.cpp
- Committer:
- shintamainjp
- Date:
- 2010-11-22
- Revision:
- 0:d825f8dae2be
File content as of revision 0:d825f8dae2be:
/** * StarBoard Orange - Example application No.3 (Version 0.0.1) * Drive a CHORO Q HYBRID with wii nunchuk * * See also ... http://mbed.org/users/shintamainjp/notebook/starboard_example3_ja/ * See also ... http://mbed.org/users/shintamainjp/notebook/starboard_example3_en/ * * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems) * http://shinta.main.jp/ */ /* * Connection map. * * +---+----------------+---------+ * |Pin|Target |Direction| * +---+----------------+---------+ * |p21|IR transmitter |OUT | * +---+----------------+---------+ */ /* * Include files. */ #include <mbed.h> #include <algorithm> #include <ChoroQ.h> #include <I2CConfig.h> #include <WiiNunchuckReader.h> #include "appconf.h" /* * Objects. */ WiiNunchuckReader wn1(I2CPort_A::SDA, I2CPort_A::SCL); WiiNunchuckReader wn2(I2CPort_B::SDA, I2CPort_B::SCL); ChoroQ cq(p21); BusOut led(LED4, LED3, LED2, LED1); /** * Display a splash screen. */ void splash(void) { // Do nothing. } /** * Get an action from a coordinate. * * @param x X axis. * @param y Y axis. * @param dash State of dash. * * @return An action. */ ChoroQ::Action getAction(const int x, const int y, bool dash) { static const int MAX_X = 200; static const int MIN_X = 20; static const int MAX_Y = 200; static const int MIN_Y = 20; int px = ((x - MIN_X) * 100) / (MAX_X - MIN_X); int py = ((y - MIN_Y) * 100) / (MAX_Y - MIN_Y); px = std::max(0, std::min(100, px)) - 50; // Range of a value is -50 to +50 py = std::max(0, std::min(100, py)) - 50; // Range of a value is -50 to +50 if ((std::abs(px) <= 10) && (std::abs(py) <= 10)) { return ChoroQ::Stop; } if ((x == 0) && (y == 0)) { return ChoroQ::Stop; } if (std::abs(px) < 10) { if (py < 0) { if (dash) { return ChoroQ::DownDash; } else { return ChoroQ::Down; } } else { if (dash) { return ChoroQ::UpDash; } else { return ChoroQ::Up; } } } if (std::abs(py) < 10) { if (px < -20) { return ChoroQ::Left; } if (20 < px) { return ChoroQ::Right; } } if ((10 < px) && (10 < py)) { if (dash) { return ChoroQ::UpRightDash; } else { return ChoroQ::UpRight; } } if ((px < -10) && (10 < py)) { if (dash) { return ChoroQ::UpLeftDash; } else { return ChoroQ::UpLeft; } } if ((px < -10) && (py < -10)) { if (dash) { return ChoroQ::DownLeftDash; } else { return ChoroQ::DownLeft; } } if ((10 < px) && (py < -10)) { if (dash) { return ChoroQ::DownRightDash; } else { return ChoroQ::DownRight; } } return ChoroQ::Stop; } /** * Entry point. */ int main() { /* * Splash. */ splash(); /* * Setup a configuration. */ appconf_t conf; appconf_init(&conf); if (appconf_read(&conf) != 0) { error("Please check the configuration."); } /* * Application loop. */ while (true) { led = led + 1; wn1.RequestRead(); ChoroQ::Action ac1 = getAction(wn1.getJoyX(), wn1.getJoyY(), (wn1.getButtonZ() == 1) ? true : false); cq.execute(conf.ch1, ac1, false); wait_ms(25); led = led + 1; wn2.RequestRead(); ChoroQ::Action ac2 = getAction(wn2.getJoyX(), wn2.getJoyY(), (wn2.getButtonZ() == 1) ? true : false); cq.execute(conf.ch2, ac2, false); wait_ms(25); } }