Shinichiro Nakamura / Mbed 2 deprecated 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?

UserRevisionLine numberNew 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 }