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

cook.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 /* cook.cpp */
Nishii 0:ae5d19a716e1 2
Nishii 0:ae5d19a716e1 3 #include "global.h"
Nishii 0:ae5d19a716e1 4
Nishii 0:ae5d19a716e1 5 /**
Nishii 0:ae5d19a716e1 6 * Parameters
Nishii 0:ae5d19a716e1 7 */
Nishii 0:ae5d19a716e1 8 int aori_times = 13;
Nishii 0:ae5d19a716e1 9
Nishii 0:ae5d19a716e1 10 double hand_interval=0.5, egg_gasya_times=6, rice_gasya_times=12;
Nishii 0:ae5d19a716e1 11 int highest=0, lowest=-750, middle=-450; //rotary encoder
Nishii 0:ae5d19a716e1 12
Nishii 0:ae5d19a716e1 13 double servetime = 9.0;
Nishii 0:ae5d19a716e1 14
Nishii 0:ae5d19a716e1 15 /**
Nishii 0:ae5d19a716e1 16 * Functions
Nishii 0:ae5d19a716e1 17 */
Nishii 0:ae5d19a716e1 18 void AORI() {
Nishii 0:ae5d19a716e1 19
Nishii 0:ae5d19a716e1 20 int top, count=0;
Nishii 0:ae5d19a716e1 21 if (MS_A.read()) {
Nishii 0:ae5d19a716e1 22 top=ON;
Nishii 0:ae5d19a716e1 23 } else {
Nishii 0:ae5d19a716e1 24 top=OFF;
Nishii 0:ae5d19a716e1 25 }
Nishii 0:ae5d19a716e1 26
Nishii 0:ae5d19a716e1 27 setspeed(duty_A, _A_);
Nishii 0:ae5d19a716e1 28 while(count<aori_times) {
Nishii 0:ae5d19a716e1 29 if(MS_A.read()!=top) {
Nishii 0:ae5d19a716e1 30 if(top==OFF) {
Nishii 0:ae5d19a716e1 31 top=ON;
Nishii 0:ae5d19a716e1 32 count++;
Nishii 0:ae5d19a716e1 33 }
Nishii 0:ae5d19a716e1 34 else {
Nishii 0:ae5d19a716e1 35 top=OFF;
Nishii 0:ae5d19a716e1 36 }
Nishii 0:ae5d19a716e1 37 }
Nishii 0:ae5d19a716e1 38 wait(0.05);
Nishii 0:ae5d19a716e1 39 }
Nishii 0:ae5d19a716e1 40 setspeed(dutyr_A, _A_);
Nishii 0:ae5d19a716e1 41 while(MS_A.read()) {
Nishii 0:ae5d19a716e1 42 wait(0.05);
Nishii 0:ae5d19a716e1 43 }
Nishii 0:ae5d19a716e1 44 setspeed(STOP, _A_);
Nishii 0:ae5d19a716e1 45 }
Nishii 0:ae5d19a716e1 46
Nishii 0:ae5d19a716e1 47 void updown(int trg) {
Nishii 0:ae5d19a716e1 48
Nishii 0:ae5d19a716e1 49 if(hight.getPulses()<trg) {
Nishii 0:ae5d19a716e1 50 setspeed(dutyu_U, _U_);
Nishii 0:ae5d19a716e1 51 if(trg==highest) {
Nishii 0:ae5d19a716e1 52
Nishii 0:ae5d19a716e1 53 while(MS_U.read()==1 && hight.getPulses()<trg) {
Nishii 0:ae5d19a716e1 54 wait(0.05);
Nishii 0:ae5d19a716e1 55 }
Nishii 0:ae5d19a716e1 56 setspeed(STOP, _U_);
Nishii 0:ae5d19a716e1 57
Nishii 0:ae5d19a716e1 58 }
Nishii 0:ae5d19a716e1 59 else{
Nishii 0:ae5d19a716e1 60 while(hight.getPulses()<trg) {
Nishii 0:ae5d19a716e1 61 wait(0.05);
Nishii 0:ae5d19a716e1 62 }
Nishii 0:ae5d19a716e1 63 setspeed(STOP, _U_);
Nishii 0:ae5d19a716e1 64 }
Nishii 0:ae5d19a716e1 65 }
Nishii 0:ae5d19a716e1 66
Nishii 0:ae5d19a716e1 67 else if(hight.getPulses()>trg) {
Nishii 0:ae5d19a716e1 68 setspeed(dutyd_U, _U_);
Nishii 0:ae5d19a716e1 69
Nishii 0:ae5d19a716e1 70 if(trg==highest) {
Nishii 0:ae5d19a716e1 71
Nishii 0:ae5d19a716e1 72 while(MS_U.read()==1 && hight.getPulses()>trg) {
Nishii 0:ae5d19a716e1 73 wait(0.05);
Nishii 0:ae5d19a716e1 74 }
Nishii 0:ae5d19a716e1 75 setspeed(STOP, _U_);
Nishii 0:ae5d19a716e1 76
Nishii 0:ae5d19a716e1 77 }
Nishii 0:ae5d19a716e1 78 else{
Nishii 0:ae5d19a716e1 79 while(hight.getPulses()>trg) {
Nishii 0:ae5d19a716e1 80 wait(0.05);
Nishii 0:ae5d19a716e1 81 }
Nishii 0:ae5d19a716e1 82 setspeed(STOP, _U_);
Nishii 0:ae5d19a716e1 83 }
Nishii 0:ae5d19a716e1 84 }
Nishii 0:ae5d19a716e1 85 }
Nishii 0:ae5d19a716e1 86
Nishii 0:ae5d19a716e1 87 void gasya ( short n ) {
Nishii 0:ae5d19a716e1 88 for( short i=0; i<n; i++ ) {
Nishii 0:ae5d19a716e1 89 UFO = CLOSE;
Nishii 0:ae5d19a716e1 90 wait(hand_interval);
Nishii 0:ae5d19a716e1 91 UFO = OPEN;
Nishii 0:ae5d19a716e1 92 wait(hand_interval);
Nishii 0:ae5d19a716e1 93 }
Nishii 0:ae5d19a716e1 94 }
Nishii 0:ae5d19a716e1 95
Nishii 0:ae5d19a716e1 96 void egg_hand_rot(int n) {
Nishii 0:ae5d19a716e1 97
Nishii 0:ae5d19a716e1 98 UFO = CLOSE;
Nishii 0:ae5d19a716e1 99 updown(lowest);
Nishii 0:ae5d19a716e1 100 wait(0.5);
Nishii 0:ae5d19a716e1 101 gasya(n);
Nishii 0:ae5d19a716e1 102 wait(0.5);
Nishii 0:ae5d19a716e1 103 UFO = CLOSE;
Nishii 0:ae5d19a716e1 104 wait(0.3);
Nishii 0:ae5d19a716e1 105 updown(highest);
Nishii 0:ae5d19a716e1 106 //UFO = CLOSE;
Nishii 0:ae5d19a716e1 107 }
Nishii 0:ae5d19a716e1 108
Nishii 0:ae5d19a716e1 109 void rice_hand_rot(int n) {
Nishii 0:ae5d19a716e1 110
Nishii 0:ae5d19a716e1 111 for (short i=0; i<n; i++) {
Nishii 0:ae5d19a716e1 112 UFO = CLOSE;
Nishii 0:ae5d19a716e1 113 updown(lowest);
Nishii 0:ae5d19a716e1 114 wait(0.3);
Nishii 0:ae5d19a716e1 115 gasya(rice_gasya_times);
Nishii 0:ae5d19a716e1 116 UFO = CLOSE;
Nishii 0:ae5d19a716e1 117 wait(0.3);
Nishii 0:ae5d19a716e1 118 updown(middle);
Nishii 0:ae5d19a716e1 119 wait(0.3);
Nishii 0:ae5d19a716e1 120 UFO = OPEN;
Nishii 0:ae5d19a716e1 121 wait(1);
Nishii 0:ae5d19a716e1 122 UFO = CLOSE;
Nishii 0:ae5d19a716e1 123 }
Nishii 0:ae5d19a716e1 124 updown(highest);
Nishii 0:ae5d19a716e1 125 }
Nishii 0:ae5d19a716e1 126
Nishii 0:ae5d19a716e1 127 void merry() {
Nishii 0:ae5d19a716e1 128 //int count;
Nishii 0:ae5d19a716e1 129 setspeed(duty_M, _M_);
Nishii 0:ae5d19a716e1 130 wait(1);
Nishii 0:ae5d19a716e1 131 while(MS_M.read()) {
Nishii 0:ae5d19a716e1 132 wait(0.1);
Nishii 0:ae5d19a716e1 133 }
Nishii 0:ae5d19a716e1 134 setspeed(STOP, _M_);
Nishii 0:ae5d19a716e1 135 }
Nishii 0:ae5d19a716e1 136
Nishii 0:ae5d19a716e1 137 void serve() {
Nishii 0:ae5d19a716e1 138 setspeed(duty_S, _S_);
Nishii 0:ae5d19a716e1 139 wait(servetime);
Nishii 0:ae5d19a716e1 140 setspeed(STOP, _S_);
Nishii 0:ae5d19a716e1 141 wait(4.0);
Nishii 0:ae5d19a716e1 142 setspeed(dutyr_S, _S_);
Nishii 0:ae5d19a716e1 143 while(MS_S.read()) {
Nishii 0:ae5d19a716e1 144 wait(0.05);
Nishii 0:ae5d19a716e1 145 }
Nishii 0:ae5d19a716e1 146 setspeed(STOP, _S_);
Nishii 0:ae5d19a716e1 147 }
Nishii 0:ae5d19a716e1 148
Nishii 0:ae5d19a716e1 149 void cook() {
Nishii 0:ae5d19a716e1 150 pc.attach(cook_stop, Serial::RxIrq);
Nishii 0:ae5d19a716e1 151 double gg=0.7;
Nishii 0:ae5d19a716e1 152 wait(1.0);
Nishii 0:ae5d19a716e1 153
Nishii 0:ae5d19a716e1 154 //merry rice
Nishii 0:ae5d19a716e1 155 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 156 pc.printf( "merry egg \r\n" );
Nishii 0:ae5d19a716e1 157 merry();
Nishii 0:ae5d19a716e1 158 wait(gg);
Nishii 0:ae5d19a716e1 159 }
Nishii 0:ae5d19a716e1 160
Nishii 0:ae5d19a716e1 161 //hand rice
Nishii 0:ae5d19a716e1 162 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 163 pc.printf( "hand \r\n" );
Nishii 0:ae5d19a716e1 164 egg_hand_rot(egg_gasya_times-2);
Nishii 0:ae5d19a716e1 165 wait(gg);
Nishii 0:ae5d19a716e1 166 }
Nishii 0:ae5d19a716e1 167
Nishii 0:ae5d19a716e1 168 //merry egg
Nishii 0:ae5d19a716e1 169 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 170 pc.printf( "merry rice \r\n" );
Nishii 0:ae5d19a716e1 171 merry();
Nishii 0:ae5d19a716e1 172 wait(gg);
Nishii 0:ae5d19a716e1 173 }
Nishii 0:ae5d19a716e1 174
Nishii 0:ae5d19a716e1 175 //hand egg
Nishii 0:ae5d19a716e1 176 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 177 pc.printf( "hand \r\n" );
Nishii 0:ae5d19a716e1 178 egg_hand_rot(egg_gasya_times*2);
Nishii 0:ae5d19a716e1 179 //rice_hand_rot(2);
Nishii 0:ae5d19a716e1 180 wait(gg);
Nishii 0:ae5d19a716e1 181 }
Nishii 0:ae5d19a716e1 182
Nishii 0:ae5d19a716e1 183 //merry, hand 3
Nishii 0:ae5d19a716e1 184 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 185 pc.printf( "merry 3 \r\n" );
Nishii 0:ae5d19a716e1 186 merry();
Nishii 0:ae5d19a716e1 187 wait(gg);
Nishii 0:ae5d19a716e1 188 pc.printf( "hand \r\n" );
Nishii 0:ae5d19a716e1 189 egg_hand_rot(egg_gasya_times);
Nishii 0:ae5d19a716e1 190 //rice_hand_rot(1);
Nishii 0:ae5d19a716e1 191 wait(gg);
Nishii 0:ae5d19a716e1 192 }
Nishii 0:ae5d19a716e1 193
Nishii 0:ae5d19a716e1 194 //merry, hand 4
Nishii 0:ae5d19a716e1 195 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 196 pc.printf( "merry 4 \r\n" );
Nishii 0:ae5d19a716e1 197 merry();
Nishii 0:ae5d19a716e1 198 wait(gg);
Nishii 0:ae5d19a716e1 199 pc.printf( "hand \r\n" );
Nishii 0:ae5d19a716e1 200 egg_hand_rot(egg_gasya_times);
Nishii 0:ae5d19a716e1 201 //rice_hand_rot(2);
Nishii 0:ae5d19a716e1 202 wait(gg);
Nishii 0:ae5d19a716e1 203 }
Nishii 0:ae5d19a716e1 204
Nishii 0:ae5d19a716e1 205 //AORI
Nishii 0:ae5d19a716e1 206 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 207 pc.printf( "aori \r\n" );
Nishii 0:ae5d19a716e1 208 RISE1 = UP;
Nishii 0:ae5d19a716e1 209 wait(3.0);
Nishii 0:ae5d19a716e1 210 AORI();
Nishii 0:ae5d19a716e1 211 wait(1);
Nishii 0:ae5d19a716e1 212 RISE1 = DOWN;
Nishii 0:ae5d19a716e1 213 wait(2);
Nishii 0:ae5d19a716e1 214 wait(gg);
Nishii 0:ae5d19a716e1 215 }
Nishii 0:ae5d19a716e1 216
Nishii 0:ae5d19a716e1 217 // serve
Nishii 0:ae5d19a716e1 218 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 219 pc.printf( "serve \r\n" );
Nishii 0:ae5d19a716e1 220 serve();
Nishii 0:ae5d19a716e1 221 wait(3.0);
Nishii 0:ae5d19a716e1 222 }
Nishii 0:ae5d19a716e1 223
Nishii 0:ae5d19a716e1 224 //end processing
Nishii 0:ae5d19a716e1 225 if(STOP_STATUS) {
Nishii 0:ae5d19a716e1 226 pc.printf( "finish!! \r\n" );
Nishii 0:ae5d19a716e1 227 }
Nishii 0:ae5d19a716e1 228 else if(!STOP_STATUS) {
Nishii 0:ae5d19a716e1 229 pc.printf( "stop \r\n" );
Nishii 0:ae5d19a716e1 230 STOP_STATUS = true;
Nishii 0:ae5d19a716e1 231 }
Nishii 0:ae5d19a716e1 232
Nishii 0:ae5d19a716e1 233 wait(0.2);
Nishii 0:ae5d19a716e1 234 pc.putc('f'); //bgm stop
Nishii 0:ae5d19a716e1 235 wait(0.2);
Nishii 0:ae5d19a716e1 236 __disable_irq(); //stop interrupt
Nishii 0:ae5d19a716e1 237 }