Daisuke Nishii / Mbed 2 deprecated RobotGrandPrix

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

control.cpp@0:ae5d19a716e1, 2014-03-05 (annotated)

Committer:
Nishii
Date:
Wed Mar 05 05:17:45 2014 +0000
Revision:
0:ae5d19a716e1
for Robot Grand Prix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nishii 0:ae5d19a716e1 1 /* control.cpp */
Nishii 0:ae5d19a716e1 2
Nishii 0:ae5d19a716e1 3 #include "global.h"
Nishii 0:ae5d19a716e1 4
Nishii 0:ae5d19a716e1 5 /**
Nishii 0:ae5d19a716e1 6 * Functions
Nishii 0:ae5d19a716e1 7 */
Nishii 0:ae5d19a716e1 8 void reset_A() {
Nishii 0:ae5d19a716e1 9
Nishii 0:ae5d19a716e1 10 pc.printf( "A " );
Nishii 0:ae5d19a716e1 11 if(MS_A.read()) {
Nishii 0:ae5d19a716e1 12 setspeed(dutyr_A, _A_);
Nishii 0:ae5d19a716e1 13 while(MS_A.read()) {
Nishii 0:ae5d19a716e1 14 wait(0.05);
Nishii 0:ae5d19a716e1 15 }
Nishii 0:ae5d19a716e1 16 }
Nishii 0:ae5d19a716e1 17 setspeed(STOP, _A_);
Nishii 0:ae5d19a716e1 18 pc.printf( "OK \r\n" );
Nishii 0:ae5d19a716e1 19 }
Nishii 0:ae5d19a716e1 20
Nishii 0:ae5d19a716e1 21 void reset_U() {
Nishii 0:ae5d19a716e1 22
Nishii 0:ae5d19a716e1 23 pc.printf( "U " );
Nishii 0:ae5d19a716e1 24 if(MS_U.read()) {
Nishii 0:ae5d19a716e1 25 setspeed(dutyu_U, _U_);
Nishii 0:ae5d19a716e1 26 while(MS_U.read()) {
Nishii 0:ae5d19a716e1 27 wait(0.05);
Nishii 0:ae5d19a716e1 28 }
Nishii 0:ae5d19a716e1 29 }
Nishii 0:ae5d19a716e1 30 setspeed(STOP, _U_);
Nishii 0:ae5d19a716e1 31 hight.reset();
Nishii 0:ae5d19a716e1 32 pc.printf( "OK \r\n" );
Nishii 0:ae5d19a716e1 33 }
Nishii 0:ae5d19a716e1 34
Nishii 0:ae5d19a716e1 35 void reset_M() {
Nishii 0:ae5d19a716e1 36
Nishii 0:ae5d19a716e1 37 pc.printf( "M " );
Nishii 0:ae5d19a716e1 38 if(MS_M.read()) {
Nishii 0:ae5d19a716e1 39 setspeed(dutyr_M, _M_);
Nishii 0:ae5d19a716e1 40 while(MS_M.read()) {
Nishii 0:ae5d19a716e1 41 wait(0.05);
Nishii 0:ae5d19a716e1 42 }
Nishii 0:ae5d19a716e1 43 }
Nishii 0:ae5d19a716e1 44 setspeed(STOP, _M_);
Nishii 0:ae5d19a716e1 45 pc.printf( "OK \r\n" );
Nishii 0:ae5d19a716e1 46 }
Nishii 0:ae5d19a716e1 47
Nishii 0:ae5d19a716e1 48 void reset_S() {
Nishii 0:ae5d19a716e1 49
Nishii 0:ae5d19a716e1 50 pc.printf( "S " );
Nishii 0:ae5d19a716e1 51 if(MS_S.read()) {
Nishii 0:ae5d19a716e1 52 setspeed(dutyr_S, _S_);
Nishii 0:ae5d19a716e1 53 while(MS_S.read()) {
Nishii 0:ae5d19a716e1 54 wait(0.01);
Nishii 0:ae5d19a716e1 55 }
Nishii 0:ae5d19a716e1 56 }
Nishii 0:ae5d19a716e1 57 setspeed(STOP, _S_);
Nishii 0:ae5d19a716e1 58 pc.printf( "OK \r\n" );
Nishii 0:ae5d19a716e1 59 }
Nishii 0:ae5d19a716e1 60
Nishii 0:ae5d19a716e1 61 void reset() {
Nishii 0:ae5d19a716e1 62
Nishii 0:ae5d19a716e1 63 reset_A();
Nishii 0:ae5d19a716e1 64 reset_U();
Nishii 0:ae5d19a716e1 65 reset_M();
Nishii 0:ae5d19a716e1 66 reset_S();
Nishii 0:ae5d19a716e1 67 }
Nishii 0:ae5d19a716e1 68
Nishii 0:ae5d19a716e1 69 void cook_stop() {
Nishii 0:ae5d19a716e1 70 if(pc.getc()=='s') {
Nishii 0:ae5d19a716e1 71 pc.printf( "wait a momnet \r\n" );
Nishii 0:ae5d19a716e1 72 ALLSTOP();
Nishii 0:ae5d19a716e1 73 pc.printf( "input \r\n" );
Nishii 0:ae5d19a716e1 74 pc.getc();
Nishii 0:ae5d19a716e1 75 STOP_STATUS = false;
Nishii 0:ae5d19a716e1 76 }
Nishii 0:ae5d19a716e1 77 }
Nishii 0:ae5d19a716e1 78
Nishii 0:ae5d19a716e1 79 void AUTO() {
Nishii 0:ae5d19a716e1 80
Nishii 0:ae5d19a716e1 81 char c;
Nishii 0:ae5d19a716e1 82 pc.printf( "A" );
Nishii 0:ae5d19a716e1 83 wait(0.5);
Nishii 0:ae5d19a716e1 84 pc.printf( "auto mode \r\n" );
Nishii 0:ae5d19a716e1 85 while(1) {
Nishii 0:ae5d19a716e1 86 c = pc.getc();
Nishii 0:ae5d19a716e1 87 if(c=='b') break;
Nishii 0:ae5d19a716e1 88 switch (c) {
Nishii 0:ae5d19a716e1 89 case 'R':
Nishii 0:ae5d19a716e1 90 pc.printf( "reset \r\n" );
Nishii 0:ae5d19a716e1 91 reset();
Nishii 0:ae5d19a716e1 92 break;
Nishii 0:ae5d19a716e1 93 case 'r':
Nishii 0:ae5d19a716e1 94 c = pc.getc();
Nishii 0:ae5d19a716e1 95 switch (c) {
Nishii 0:ae5d19a716e1 96 case 'A':
Nishii 0:ae5d19a716e1 97 reset_A();
Nishii 0:ae5d19a716e1 98 break;
Nishii 0:ae5d19a716e1 99 case 'U':
Nishii 0:ae5d19a716e1 100 reset_U();
Nishii 0:ae5d19a716e1 101 break;
Nishii 0:ae5d19a716e1 102 case 'M':
Nishii 0:ae5d19a716e1 103 reset_M();
Nishii 0:ae5d19a716e1 104 break;
Nishii 0:ae5d19a716e1 105 case 'S':
Nishii 0:ae5d19a716e1 106 reset_S();
Nishii 0:ae5d19a716e1 107 break;
Nishii 0:ae5d19a716e1 108 }
Nishii 0:ae5d19a716e1 109 break;
Nishii 0:ae5d19a716e1 110 case 'f':
Nishii 0:ae5d19a716e1 111 pc.printf( "start \r\n" );
Nishii 0:ae5d19a716e1 112 cook();
Nishii 0:ae5d19a716e1 113 //pc.printf( "finish \r\n" );
Nishii 0:ae5d19a716e1 114 break;
Nishii 0:ae5d19a716e1 115 }
Nishii 0:ae5d19a716e1 116 }
Nishii 0:ae5d19a716e1 117 }
Nishii 0:ae5d19a716e1 118
Nishii 0:ae5d19a716e1 119 void MANUAL() {
Nishii 0:ae5d19a716e1 120
Nishii 0:ae5d19a716e1 121 char c;
Nishii 0:ae5d19a716e1 122 double step;
Nishii 0:ae5d19a716e1 123 pc.printf( "M" );
Nishii 0:ae5d19a716e1 124 wait(0.5);
Nishii 0:ae5d19a716e1 125 pc.printf( "manual mode \r\n" );
Nishii 0:ae5d19a716e1 126 while(1) {
Nishii 0:ae5d19a716e1 127 c = pc.getc();
Nishii 0:ae5d19a716e1 128 if(c=='b') {
Nishii 0:ae5d19a716e1 129 ALLSTOP();
Nishii 0:ae5d19a716e1 130 pc.printf( "all stop \r\n" );
Nishii 0:ae5d19a716e1 131 break;
Nishii 0:ae5d19a716e1 132 }
Nishii 0:ae5d19a716e1 133 else if(c=='s') {
Nishii 0:ae5d19a716e1 134 ALLSTOP();
Nishii 0:ae5d19a716e1 135 led_all(OFF);
Nishii 0:ae5d19a716e1 136 pc.printf( "all stop \r\n" );
Nishii 0:ae5d19a716e1 137 continue;
Nishii 0:ae5d19a716e1 138 }
Nishii 0:ae5d19a716e1 139 else if(c=='w') {
Nishii 0:ae5d19a716e1 140 step = 0.05;
Nishii 0:ae5d19a716e1 141 c = pc.getc();
Nishii 0:ae5d19a716e1 142 }
Nishii 0:ae5d19a716e1 143 else if(c=='n') {
Nishii 0:ae5d19a716e1 144 step = 0.01;
Nishii 0:ae5d19a716e1 145 c = pc.getc();
Nishii 0:ae5d19a716e1 146 }
Nishii 0:ae5d19a716e1 147 switch (c) {
Nishii 0:ae5d19a716e1 148 case 'a':
Nishii 0:ae5d19a716e1 149 speed[_A_]+=step;
Nishii 0:ae5d19a716e1 150 mortar_A = speed[_A_];
Nishii 0:ae5d19a716e1 151 if (mortar_A.read() == STOP) led1=OFF;
Nishii 0:ae5d19a716e1 152 else led1=ON;
Nishii 0:ae5d19a716e1 153 pc.printf( "mortar_A : %3f \r\n", 1.0-mortar_A.read() );
Nishii 0:ae5d19a716e1 154 break;
Nishii 0:ae5d19a716e1 155 case 'A':
Nishii 0:ae5d19a716e1 156 speed[_A_]-=step;
Nishii 0:ae5d19a716e1 157 mortar_A = speed[_A_];
Nishii 0:ae5d19a716e1 158 if (mortar_A.read() == STOP) led1=OFF;
Nishii 0:ae5d19a716e1 159 else led1=ON;
Nishii 0:ae5d19a716e1 160 pc.printf( "mortar_A : %3f \r\n", 1.0-mortar_A.read() );
Nishii 0:ae5d19a716e1 161 break;
Nishii 0:ae5d19a716e1 162 case 'U':
Nishii 0:ae5d19a716e1 163 speed[_U_]+=step;
Nishii 0:ae5d19a716e1 164 mortar_U = speed[_U_];
Nishii 0:ae5d19a716e1 165 if (mortar_U.read() == STOP) led2=OFF;
Nishii 0:ae5d19a716e1 166 else led2=ON;
Nishii 0:ae5d19a716e1 167 pc.printf( "mortar_U : %3f \r\n", mortar_U.read() );
Nishii 0:ae5d19a716e1 168 pc.printf( "%d \r\n", hight.getPulses() );
Nishii 0:ae5d19a716e1 169 break;
Nishii 0:ae5d19a716e1 170 case 'u':
Nishii 0:ae5d19a716e1 171 speed[_U_]-=step;
Nishii 0:ae5d19a716e1 172 mortar_U = speed[_U_];
Nishii 0:ae5d19a716e1 173 if (mortar_U.read() == STOP) led2=OFF;
Nishii 0:ae5d19a716e1 174 else led2=ON;
Nishii 0:ae5d19a716e1 175 pc.printf( "mortar_U : %3f \r\n", mortar_U.read() );
Nishii 0:ae5d19a716e1 176 pc.printf( "%d \r\n", hight.getPulses() );
Nishii 0:ae5d19a716e1 177 break;
Nishii 0:ae5d19a716e1 178 case 'm':
Nishii 0:ae5d19a716e1 179 speed[_M_]+=step;
Nishii 0:ae5d19a716e1 180 mortar_M = speed[_M_];
Nishii 0:ae5d19a716e1 181 if (mortar_M.read() == STOP) led3=OFF;
Nishii 0:ae5d19a716e1 182 else led3=ON;
Nishii 0:ae5d19a716e1 183 pc.printf( "mortar_M : %3f \r\n", 1.0-mortar_M.read() );
Nishii 0:ae5d19a716e1 184 break;
Nishii 0:ae5d19a716e1 185 case 'M' :
Nishii 0:ae5d19a716e1 186 speed[_M_]-=step;
Nishii 0:ae5d19a716e1 187 mortar_M = speed[_M_];
Nishii 0:ae5d19a716e1 188 if (mortar_M.read() == STOP) led3=OFF;
Nishii 0:ae5d19a716e1 189 else led3=ON;
Nishii 0:ae5d19a716e1 190 pc.printf( "mortar_M : %3f \r\n", 1.0-mortar_M.read() );
Nishii 0:ae5d19a716e1 191 break;
Nishii 0:ae5d19a716e1 192 case 's':
Nishii 0:ae5d19a716e1 193 speed[_S_]+=step;
Nishii 0:ae5d19a716e1 194 mortar_S = speed[_S_];
Nishii 0:ae5d19a716e1 195 if (mortar_S.read() == STOP) led4=OFF;
Nishii 0:ae5d19a716e1 196 else led4=ON;
Nishii 0:ae5d19a716e1 197 pc.printf( "mortar_S : %3f \r\n", 1.0-mortar_S.read() );
Nishii 0:ae5d19a716e1 198 break;
Nishii 0:ae5d19a716e1 199 case 'S':
Nishii 0:ae5d19a716e1 200 speed[_S_]-=step;
Nishii 0:ae5d19a716e1 201 mortar_S = speed[_S_];
Nishii 0:ae5d19a716e1 202 if (mortar_S.read() == STOP) led4=OFF;
Nishii 0:ae5d19a716e1 203 else led4=ON;
Nishii 0:ae5d19a716e1 204 pc.printf( "mortar_S : %3f \r\n", 1.0-mortar_S.read() );
Nishii 0:ae5d19a716e1 205 break;
Nishii 0:ae5d19a716e1 206 case 'H':
Nishii 0:ae5d19a716e1 207 UFO = ON;
Nishii 0:ae5d19a716e1 208 pc.printf( "UFO : ON \r\n" );
Nishii 0:ae5d19a716e1 209 break;
Nishii 0:ae5d19a716e1 210 case 'h':
Nishii 0:ae5d19a716e1 211 UFO = OFF;
Nishii 0:ae5d19a716e1 212 pc.printf( "UFO : OFF \r\n" );
Nishii 0:ae5d19a716e1 213 break;
Nishii 0:ae5d19a716e1 214 case 'R':
Nishii 0:ae5d19a716e1 215 RISE1 = UP;
Nishii 0:ae5d19a716e1 216 pc.printf( "up \r\n" );
Nishii 0:ae5d19a716e1 217 break;
Nishii 0:ae5d19a716e1 218 case 'r':
Nishii 0:ae5d19a716e1 219 RISE1 = DOWN;
Nishii 0:ae5d19a716e1 220 pc.printf( "down \r\n" );
Nishii 0:ae5d19a716e1 221 break;
Nishii 0:ae5d19a716e1 222 }
Nishii 0:ae5d19a716e1 223 }
Nishii 0:ae5d19a716e1 224 }
Nishii 0:ae5d19a716e1 225
Nishii 0:ae5d19a716e1 226 void semi() {
Nishii 0:ae5d19a716e1 227
Nishii 0:ae5d19a716e1 228 char c=pc.getc();
Nishii 0:ae5d19a716e1 229 if(c=='E') {
Nishii 0:ae5d19a716e1 230 pc.printf( "wait \r\n" );
Nishii 0:ae5d19a716e1 231 ALLSTOP();
Nishii 0:ae5d19a716e1 232 pc.printf( "stop \r\n" );
Nishii 0:ae5d19a716e1 233 pc.printf( "input \r\n" );
Nishii 0:ae5d19a716e1 234 pc.getc();
Nishii 0:ae5d19a716e1 235 }
Nishii 0:ae5d19a716e1 236 }
Nishii 0:ae5d19a716e1 237
Nishii 0:ae5d19a716e1 238 void SEMIAUTO() {
Nishii 0:ae5d19a716e1 239
Nishii 0:ae5d19a716e1 240 char c;
Nishii 0:ae5d19a716e1 241
Nishii 0:ae5d19a716e1 242 pc.printf( "S" );
Nishii 0:ae5d19a716e1 243 wait(0.5);
Nishii 0:ae5d19a716e1 244 pc.printf( "semiauto mode \r\n" );
Nishii 0:ae5d19a716e1 245 while(1) {
Nishii 0:ae5d19a716e1 246 c = pc.getc();
Nishii 0:ae5d19a716e1 247 if (c=='b') break;
Nishii 0:ae5d19a716e1 248 pc.attach(semi, Serial::RxIrq);
Nishii 0:ae5d19a716e1 249 switch (c) {
Nishii 0:ae5d19a716e1 250 case 'A':
Nishii 0:ae5d19a716e1 251 AORI();
Nishii 0:ae5d19a716e1 252 break;
Nishii 0:ae5d19a716e1 253 case 'U':
Nishii 0:ae5d19a716e1 254 egg_hand_rot(egg_gasya_times);
Nishii 0:ae5d19a716e1 255 break;
Nishii 0:ae5d19a716e1 256 case 'u':
Nishii 0:ae5d19a716e1 257 rice_hand_rot(2);
Nishii 0:ae5d19a716e1 258 break;
Nishii 0:ae5d19a716e1 259 case 'M':
Nishii 0:ae5d19a716e1 260 merry();
Nishii 0:ae5d19a716e1 261 break;
Nishii 0:ae5d19a716e1 262 case 'S':
Nishii 0:ae5d19a716e1 263 serve();
Nishii 0:ae5d19a716e1 264 break;
Nishii 0:ae5d19a716e1 265 }
Nishii 0:ae5d19a716e1 266 __disable_irq();
Nishii 0:ae5d19a716e1 267 pc.printf( "OK \r\n" );
Nishii 0:ae5d19a716e1 268 }
Nishii 0:ae5d19a716e1 269 }