Shinichiro Nakamura
/
Drive2ChoroQ
This is a demonstration of two Choro Q Hybrid cars.
main.cpp@0:d825f8dae2be, 2010-11-22 (annotated)
- Committer:
- shintamainjp
- Date:
- Mon Nov 22 12:23:23 2010 +0000
- Revision:
- 0:d825f8dae2be
Initial version.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shintamainjp | 0:d825f8dae2be | 1 | /** |
shintamainjp | 0:d825f8dae2be | 2 | * StarBoard Orange - Example application No.3 (Version 0.0.1) |
shintamainjp | 0:d825f8dae2be | 3 | * Drive a CHORO Q HYBRID with wii nunchuk |
shintamainjp | 0:d825f8dae2be | 4 | * |
shintamainjp | 0:d825f8dae2be | 5 | * See also ... http://mbed.org/users/shintamainjp/notebook/starboard_example3_ja/ |
shintamainjp | 0:d825f8dae2be | 6 | * See also ... http://mbed.org/users/shintamainjp/notebook/starboard_example3_en/ |
shintamainjp | 0:d825f8dae2be | 7 | * |
shintamainjp | 0:d825f8dae2be | 8 | * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems) |
shintamainjp | 0:d825f8dae2be | 9 | * http://shinta.main.jp/ |
shintamainjp | 0:d825f8dae2be | 10 | */ |
shintamainjp | 0:d825f8dae2be | 11 | |
shintamainjp | 0:d825f8dae2be | 12 | /* |
shintamainjp | 0:d825f8dae2be | 13 | * Connection map. |
shintamainjp | 0:d825f8dae2be | 14 | * |
shintamainjp | 0:d825f8dae2be | 15 | * +---+----------------+---------+ |
shintamainjp | 0:d825f8dae2be | 16 | * |Pin|Target |Direction| |
shintamainjp | 0:d825f8dae2be | 17 | * +---+----------------+---------+ |
shintamainjp | 0:d825f8dae2be | 18 | * |p21|IR transmitter |OUT | |
shintamainjp | 0:d825f8dae2be | 19 | * +---+----------------+---------+ |
shintamainjp | 0:d825f8dae2be | 20 | */ |
shintamainjp | 0:d825f8dae2be | 21 | |
shintamainjp | 0:d825f8dae2be | 22 | /* |
shintamainjp | 0:d825f8dae2be | 23 | * Include files. |
shintamainjp | 0:d825f8dae2be | 24 | */ |
shintamainjp | 0:d825f8dae2be | 25 | |
shintamainjp | 0:d825f8dae2be | 26 | #include <mbed.h> |
shintamainjp | 0:d825f8dae2be | 27 | #include <algorithm> |
shintamainjp | 0:d825f8dae2be | 28 | #include <ChoroQ.h> |
shintamainjp | 0:d825f8dae2be | 29 | #include <I2CConfig.h> |
shintamainjp | 0:d825f8dae2be | 30 | #include <WiiNunchuckReader.h> |
shintamainjp | 0:d825f8dae2be | 31 | #include "appconf.h" |
shintamainjp | 0:d825f8dae2be | 32 | |
shintamainjp | 0:d825f8dae2be | 33 | /* |
shintamainjp | 0:d825f8dae2be | 34 | * Objects. |
shintamainjp | 0:d825f8dae2be | 35 | */ |
shintamainjp | 0:d825f8dae2be | 36 | |
shintamainjp | 0:d825f8dae2be | 37 | WiiNunchuckReader wn1(I2CPort_A::SDA, I2CPort_A::SCL); |
shintamainjp | 0:d825f8dae2be | 38 | WiiNunchuckReader wn2(I2CPort_B::SDA, I2CPort_B::SCL); |
shintamainjp | 0:d825f8dae2be | 39 | ChoroQ cq(p21); |
shintamainjp | 0:d825f8dae2be | 40 | BusOut led(LED4, LED3, LED2, LED1); |
shintamainjp | 0:d825f8dae2be | 41 | |
shintamainjp | 0:d825f8dae2be | 42 | /** |
shintamainjp | 0:d825f8dae2be | 43 | * Display a splash screen. |
shintamainjp | 0:d825f8dae2be | 44 | */ |
shintamainjp | 0:d825f8dae2be | 45 | void splash(void) { |
shintamainjp | 0:d825f8dae2be | 46 | // Do nothing. |
shintamainjp | 0:d825f8dae2be | 47 | } |
shintamainjp | 0:d825f8dae2be | 48 | |
shintamainjp | 0:d825f8dae2be | 49 | /** |
shintamainjp | 0:d825f8dae2be | 50 | * Get an action from a coordinate. |
shintamainjp | 0:d825f8dae2be | 51 | * |
shintamainjp | 0:d825f8dae2be | 52 | * @param x X axis. |
shintamainjp | 0:d825f8dae2be | 53 | * @param y Y axis. |
shintamainjp | 0:d825f8dae2be | 54 | * @param dash State of dash. |
shintamainjp | 0:d825f8dae2be | 55 | * |
shintamainjp | 0:d825f8dae2be | 56 | * @return An action. |
shintamainjp | 0:d825f8dae2be | 57 | */ |
shintamainjp | 0:d825f8dae2be | 58 | ChoroQ::Action getAction(const int x, const int y, bool dash) { |
shintamainjp | 0:d825f8dae2be | 59 | static const int MAX_X = 200; |
shintamainjp | 0:d825f8dae2be | 60 | static const int MIN_X = 20; |
shintamainjp | 0:d825f8dae2be | 61 | static const int MAX_Y = 200; |
shintamainjp | 0:d825f8dae2be | 62 | static const int MIN_Y = 20; |
shintamainjp | 0:d825f8dae2be | 63 | int px = ((x - MIN_X) * 100) / (MAX_X - MIN_X); |
shintamainjp | 0:d825f8dae2be | 64 | int py = ((y - MIN_Y) * 100) / (MAX_Y - MIN_Y); |
shintamainjp | 0:d825f8dae2be | 65 | px = std::max(0, std::min(100, px)) - 50; // Range of a value is -50 to +50 |
shintamainjp | 0:d825f8dae2be | 66 | py = std::max(0, std::min(100, py)) - 50; // Range of a value is -50 to +50 |
shintamainjp | 0:d825f8dae2be | 67 | |
shintamainjp | 0:d825f8dae2be | 68 | if ((std::abs(px) <= 10) && (std::abs(py) <= 10)) { |
shintamainjp | 0:d825f8dae2be | 69 | return ChoroQ::Stop; |
shintamainjp | 0:d825f8dae2be | 70 | } |
shintamainjp | 0:d825f8dae2be | 71 | |
shintamainjp | 0:d825f8dae2be | 72 | if ((x == 0) && (y == 0)) { |
shintamainjp | 0:d825f8dae2be | 73 | return ChoroQ::Stop; |
shintamainjp | 0:d825f8dae2be | 74 | } |
shintamainjp | 0:d825f8dae2be | 75 | |
shintamainjp | 0:d825f8dae2be | 76 | if (std::abs(px) < 10) { |
shintamainjp | 0:d825f8dae2be | 77 | if (py < 0) { |
shintamainjp | 0:d825f8dae2be | 78 | if (dash) { |
shintamainjp | 0:d825f8dae2be | 79 | return ChoroQ::DownDash; |
shintamainjp | 0:d825f8dae2be | 80 | } else { |
shintamainjp | 0:d825f8dae2be | 81 | return ChoroQ::Down; |
shintamainjp | 0:d825f8dae2be | 82 | } |
shintamainjp | 0:d825f8dae2be | 83 | } else { |
shintamainjp | 0:d825f8dae2be | 84 | if (dash) { |
shintamainjp | 0:d825f8dae2be | 85 | return ChoroQ::UpDash; |
shintamainjp | 0:d825f8dae2be | 86 | } else { |
shintamainjp | 0:d825f8dae2be | 87 | return ChoroQ::Up; |
shintamainjp | 0:d825f8dae2be | 88 | } |
shintamainjp | 0:d825f8dae2be | 89 | } |
shintamainjp | 0:d825f8dae2be | 90 | } |
shintamainjp | 0:d825f8dae2be | 91 | if (std::abs(py) < 10) { |
shintamainjp | 0:d825f8dae2be | 92 | if (px < -20) { |
shintamainjp | 0:d825f8dae2be | 93 | return ChoroQ::Left; |
shintamainjp | 0:d825f8dae2be | 94 | } |
shintamainjp | 0:d825f8dae2be | 95 | if (20 < px) { |
shintamainjp | 0:d825f8dae2be | 96 | return ChoroQ::Right; |
shintamainjp | 0:d825f8dae2be | 97 | } |
shintamainjp | 0:d825f8dae2be | 98 | } |
shintamainjp | 0:d825f8dae2be | 99 | if ((10 < px) && (10 < py)) { |
shintamainjp | 0:d825f8dae2be | 100 | if (dash) { |
shintamainjp | 0:d825f8dae2be | 101 | return ChoroQ::UpRightDash; |
shintamainjp | 0:d825f8dae2be | 102 | } else { |
shintamainjp | 0:d825f8dae2be | 103 | return ChoroQ::UpRight; |
shintamainjp | 0:d825f8dae2be | 104 | } |
shintamainjp | 0:d825f8dae2be | 105 | } |
shintamainjp | 0:d825f8dae2be | 106 | if ((px < -10) && (10 < py)) { |
shintamainjp | 0:d825f8dae2be | 107 | if (dash) { |
shintamainjp | 0:d825f8dae2be | 108 | return ChoroQ::UpLeftDash; |
shintamainjp | 0:d825f8dae2be | 109 | } else { |
shintamainjp | 0:d825f8dae2be | 110 | return ChoroQ::UpLeft; |
shintamainjp | 0:d825f8dae2be | 111 | } |
shintamainjp | 0:d825f8dae2be | 112 | } |
shintamainjp | 0:d825f8dae2be | 113 | if ((px < -10) && (py < -10)) { |
shintamainjp | 0:d825f8dae2be | 114 | if (dash) { |
shintamainjp | 0:d825f8dae2be | 115 | return ChoroQ::DownLeftDash; |
shintamainjp | 0:d825f8dae2be | 116 | } else { |
shintamainjp | 0:d825f8dae2be | 117 | return ChoroQ::DownLeft; |
shintamainjp | 0:d825f8dae2be | 118 | } |
shintamainjp | 0:d825f8dae2be | 119 | } |
shintamainjp | 0:d825f8dae2be | 120 | if ((10 < px) && (py < -10)) { |
shintamainjp | 0:d825f8dae2be | 121 | if (dash) { |
shintamainjp | 0:d825f8dae2be | 122 | return ChoroQ::DownRightDash; |
shintamainjp | 0:d825f8dae2be | 123 | } else { |
shintamainjp | 0:d825f8dae2be | 124 | return ChoroQ::DownRight; |
shintamainjp | 0:d825f8dae2be | 125 | } |
shintamainjp | 0:d825f8dae2be | 126 | } |
shintamainjp | 0:d825f8dae2be | 127 | return ChoroQ::Stop; |
shintamainjp | 0:d825f8dae2be | 128 | } |
shintamainjp | 0:d825f8dae2be | 129 | |
shintamainjp | 0:d825f8dae2be | 130 | /** |
shintamainjp | 0:d825f8dae2be | 131 | * Entry point. |
shintamainjp | 0:d825f8dae2be | 132 | */ |
shintamainjp | 0:d825f8dae2be | 133 | int main() { |
shintamainjp | 0:d825f8dae2be | 134 | /* |
shintamainjp | 0:d825f8dae2be | 135 | * Splash. |
shintamainjp | 0:d825f8dae2be | 136 | */ |
shintamainjp | 0:d825f8dae2be | 137 | splash(); |
shintamainjp | 0:d825f8dae2be | 138 | |
shintamainjp | 0:d825f8dae2be | 139 | /* |
shintamainjp | 0:d825f8dae2be | 140 | * Setup a configuration. |
shintamainjp | 0:d825f8dae2be | 141 | */ |
shintamainjp | 0:d825f8dae2be | 142 | appconf_t conf; |
shintamainjp | 0:d825f8dae2be | 143 | appconf_init(&conf); |
shintamainjp | 0:d825f8dae2be | 144 | if (appconf_read(&conf) != 0) { |
shintamainjp | 0:d825f8dae2be | 145 | error("Please check the configuration."); |
shintamainjp | 0:d825f8dae2be | 146 | } |
shintamainjp | 0:d825f8dae2be | 147 | |
shintamainjp | 0:d825f8dae2be | 148 | /* |
shintamainjp | 0:d825f8dae2be | 149 | * Application loop. |
shintamainjp | 0:d825f8dae2be | 150 | */ |
shintamainjp | 0:d825f8dae2be | 151 | while (true) { |
shintamainjp | 0:d825f8dae2be | 152 | led = led + 1; |
shintamainjp | 0:d825f8dae2be | 153 | wn1.RequestRead(); |
shintamainjp | 0:d825f8dae2be | 154 | ChoroQ::Action ac1 = getAction(wn1.getJoyX(), wn1.getJoyY(), (wn1.getButtonZ() == 1) ? true : false); |
shintamainjp | 0:d825f8dae2be | 155 | cq.execute(conf.ch1, ac1, false); |
shintamainjp | 0:d825f8dae2be | 156 | wait_ms(25); |
shintamainjp | 0:d825f8dae2be | 157 | |
shintamainjp | 0:d825f8dae2be | 158 | led = led + 1; |
shintamainjp | 0:d825f8dae2be | 159 | wn2.RequestRead(); |
shintamainjp | 0:d825f8dae2be | 160 | ChoroQ::Action ac2 = getAction(wn2.getJoyX(), wn2.getJoyY(), (wn2.getButtonZ() == 1) ? true : false); |
shintamainjp | 0:d825f8dae2be | 161 | cq.execute(conf.ch2, ac2, false); |
shintamainjp | 0:d825f8dae2be | 162 | wait_ms(25); |
shintamainjp | 0:d825f8dae2be | 163 | } |
shintamainjp | 0:d825f8dae2be | 164 | } |