Daisuke Nishii
/
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.
Diff: control.cpp
- Revision:
- 0:ae5d19a716e1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,269 @@
+/* control.cpp */
+
+#include "global.h"
+
+/**
+ * Functions
+ */
+void reset_A() {
+
+ pc.printf( "A " );
+ if(MS_A.read()) {
+ setspeed(dutyr_A, _A_);
+ while(MS_A.read()) {
+ wait(0.05);
+ }
+ }
+ setspeed(STOP, _A_);
+ pc.printf( "OK \r\n" );
+}
+
+void reset_U() {
+
+ pc.printf( "U " );
+ if(MS_U.read()) {
+ setspeed(dutyu_U, _U_);
+ while(MS_U.read()) {
+ wait(0.05);
+ }
+ }
+ setspeed(STOP, _U_);
+ hight.reset();
+ pc.printf( "OK \r\n" );
+}
+
+void reset_M() {
+
+ pc.printf( "M " );
+ if(MS_M.read()) {
+ setspeed(dutyr_M, _M_);
+ while(MS_M.read()) {
+ wait(0.05);
+ }
+ }
+ setspeed(STOP, _M_);
+ pc.printf( "OK \r\n" );
+}
+
+void reset_S() {
+
+ pc.printf( "S " );
+ if(MS_S.read()) {
+ setspeed(dutyr_S, _S_);
+ while(MS_S.read()) {
+ wait(0.01);
+ }
+ }
+ setspeed(STOP, _S_);
+ pc.printf( "OK \r\n" );
+}
+
+void reset() {
+
+ reset_A();
+ reset_U();
+ reset_M();
+ reset_S();
+}
+
+void cook_stop() {
+ if(pc.getc()=='s') {
+ pc.printf( "wait a momnet \r\n" );
+ ALLSTOP();
+ pc.printf( "input \r\n" );
+ pc.getc();
+ STOP_STATUS = false;
+ }
+}
+
+void AUTO() {
+
+ char c;
+ pc.printf( "A" );
+ wait(0.5);
+ pc.printf( "auto mode \r\n" );
+ while(1) {
+ c = pc.getc();
+ if(c=='b') break;
+ switch (c) {
+ case 'R':
+ pc.printf( "reset \r\n" );
+ reset();
+ break;
+ case 'r':
+ c = pc.getc();
+ switch (c) {
+ case 'A':
+ reset_A();
+ break;
+ case 'U':
+ reset_U();
+ break;
+ case 'M':
+ reset_M();
+ break;
+ case 'S':
+ reset_S();
+ break;
+ }
+ break;
+ case 'f':
+ pc.printf( "start \r\n" );
+ cook();
+ //pc.printf( "finish \r\n" );
+ break;
+ }
+ }
+}
+
+void MANUAL() {
+
+ char c;
+ double step;
+ pc.printf( "M" );
+ wait(0.5);
+ pc.printf( "manual mode \r\n" );
+ while(1) {
+ c = pc.getc();
+ if(c=='b') {
+ ALLSTOP();
+ pc.printf( "all stop \r\n" );
+ break;
+ }
+ else if(c=='s') {
+ ALLSTOP();
+ led_all(OFF);
+ pc.printf( "all stop \r\n" );
+ continue;
+ }
+ else if(c=='w') {
+ step = 0.05;
+ c = pc.getc();
+ }
+ else if(c=='n') {
+ step = 0.01;
+ c = pc.getc();
+ }
+ switch (c) {
+ case 'a':
+ speed[_A_]+=step;
+ mortar_A = speed[_A_];
+ if (mortar_A.read() == STOP) led1=OFF;
+ else led1=ON;
+ pc.printf( "mortar_A : %3f \r\n", 1.0-mortar_A.read() );
+ break;
+ case 'A':
+ speed[_A_]-=step;
+ mortar_A = speed[_A_];
+ if (mortar_A.read() == STOP) led1=OFF;
+ else led1=ON;
+ pc.printf( "mortar_A : %3f \r\n", 1.0-mortar_A.read() );
+ break;
+ case 'U':
+ speed[_U_]+=step;
+ mortar_U = speed[_U_];
+ if (mortar_U.read() == STOP) led2=OFF;
+ else led2=ON;
+ pc.printf( "mortar_U : %3f \r\n", mortar_U.read() );
+ pc.printf( "%d \r\n", hight.getPulses() );
+ break;
+ case 'u':
+ speed[_U_]-=step;
+ mortar_U = speed[_U_];
+ if (mortar_U.read() == STOP) led2=OFF;
+ else led2=ON;
+ pc.printf( "mortar_U : %3f \r\n", mortar_U.read() );
+ pc.printf( "%d \r\n", hight.getPulses() );
+ break;
+ case 'm':
+ speed[_M_]+=step;
+ mortar_M = speed[_M_];
+ if (mortar_M.read() == STOP) led3=OFF;
+ else led3=ON;
+ pc.printf( "mortar_M : %3f \r\n", 1.0-mortar_M.read() );
+ break;
+ case 'M' :
+ speed[_M_]-=step;
+ mortar_M = speed[_M_];
+ if (mortar_M.read() == STOP) led3=OFF;
+ else led3=ON;
+ pc.printf( "mortar_M : %3f \r\n", 1.0-mortar_M.read() );
+ break;
+ case 's':
+ speed[_S_]+=step;
+ mortar_S = speed[_S_];
+ if (mortar_S.read() == STOP) led4=OFF;
+ else led4=ON;
+ pc.printf( "mortar_S : %3f \r\n", 1.0-mortar_S.read() );
+ break;
+ case 'S':
+ speed[_S_]-=step;
+ mortar_S = speed[_S_];
+ if (mortar_S.read() == STOP) led4=OFF;
+ else led4=ON;
+ pc.printf( "mortar_S : %3f \r\n", 1.0-mortar_S.read() );
+ break;
+ case 'H':
+ UFO = ON;
+ pc.printf( "UFO : ON \r\n" );
+ break;
+ case 'h':
+ UFO = OFF;
+ pc.printf( "UFO : OFF \r\n" );
+ break;
+ case 'R':
+ RISE1 = UP;
+ pc.printf( "up \r\n" );
+ break;
+ case 'r':
+ RISE1 = DOWN;
+ pc.printf( "down \r\n" );
+ break;
+ }
+ }
+}
+
+void semi() {
+
+ char c=pc.getc();
+ if(c=='E') {
+ pc.printf( "wait \r\n" );
+ ALLSTOP();
+ pc.printf( "stop \r\n" );
+ pc.printf( "input \r\n" );
+ pc.getc();
+ }
+}
+
+void SEMIAUTO() {
+
+ char c;
+
+ pc.printf( "S" );
+ wait(0.5);
+ pc.printf( "semiauto mode \r\n" );
+ while(1) {
+ c = pc.getc();
+ if (c=='b') break;
+ pc.attach(semi, Serial::RxIrq);
+ switch (c) {
+ case 'A':
+ AORI();
+ break;
+ case 'U':
+ egg_hand_rot(egg_gasya_times);
+ break;
+ case 'u':
+ rice_hand_rot(2);
+ break;
+ case 'M':
+ merry();
+ break;
+ case 'S':
+ serve();
+ break;
+ }
+ __disable_irq();
+ pc.printf( "OK \r\n" );
+ }
+}