圭祐 杉浦 / Mbed 2 deprecated final-left

本番用候補

Dependencies:   MotorDriver_SU USBDevice mbed

final.cpp@0:0db8fdcf026f, 2014-11-28 (annotated)

Committer:
12f2027a
Date:
Fri Nov 28 11:09:34 2014 +0000
Revision:
0:0db8fdcf026f
final ??????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12f2027a 0:0db8fdcf026f 1 #include "mbed.h"
12f2027a 0:0db8fdcf026f 2 #include "MotorDriver_SU.h"
12f2027a 0:0db8fdcf026f 3
12f2027a 0:0db8fdcf026f 4
12f2027a 0:0db8fdcf026f 5 DigitalIn sw(P0_1, PullUp);
12f2027a 0:0db8fdcf026f 6 DigitalOut led(P1_28);
12f2027a 0:0db8fdcf026f 7 AnalogIn sw1(P0_22);
12f2027a 0:0db8fdcf026f 8 AnalogIn sw2(P0_12);
12f2027a 0:0db8fdcf026f 9 AnalogIn sw3(P0_16);
12f2027a 0:0db8fdcf026f 10 AnalogIn sw4(P0_11);
12f2027a 0:0db8fdcf026f 11 AnalogIn sw5(P0_13);
12f2027a 0:0db8fdcf026f 12
12f2027a 0:0db8fdcf026f 13 MotorDriver_SU motor(MOTOR_DOUBLE);
12f2027a 0:0db8fdcf026f 14 MotorDriver_SU lift(MOTOR_SINGLE);
12f2027a 0:0db8fdcf026f 15
12f2027a 0:0db8fdcf026f 16
12f2027a 0:0db8fdcf026f 17 int main( ){
12f2027a 0:0db8fdcf026f 18 float hoge1;
12f2027a 0:0db8fdcf026f 19 float hoge2;
12f2027a 0:0db8fdcf026f 20 float hoge3;
12f2027a 0:0db8fdcf026f 21 float hoge4;
12f2027a 0:0db8fdcf026f 22 float hoge5;
12f2027a 0:0db8fdcf026f 23 int position = 0;
12f2027a 0:0db8fdcf026f 24 int AI = 0;//カーブの方向判定
12f2027a 0:0db8fdcf026f 25 int updown = 0;//リフトの向き判定
12f2027a 0:0db8fdcf026f 26 int R = 0;
12f2027a 0:0db8fdcf026f 27 int L = 0;
12f2027a 0:0db8fdcf026f 28
12f2027a 0:0db8fdcf026f 29 while(1){
12f2027a 0:0db8fdcf026f 30
12f2027a 0:0db8fdcf026f 31 hoge1 = sw1.read();
12f2027a 0:0db8fdcf026f 32 hoge2 = sw2.read();
12f2027a 0:0db8fdcf026f 33 hoge3 = sw3.read();
12f2027a 0:0db8fdcf026f 34 hoge4 = sw4.read();
12f2027a 0:0db8fdcf026f 35 hoge5 = sw5.read();
12f2027a 0:0db8fdcf026f 36
12f2027a 0:0db8fdcf026f 37 while(sw == 0){
12f2027a 0:0db8fdcf026f 38 motor.Drive(0, STOP, 0);
12f2027a 0:0db8fdcf026f 39 motor.Drive(1, STOP, 0);
12f2027a 0:0db8fdcf026f 40 }
12f2027a 0:0db8fdcf026f 41 /*黒＿白＿白＿白＿白*/
12f2027a 0:0db8fdcf026f 42 if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:0db8fdcf026f 43 motor.Drive(0, CW, 500);
12f2027a 0:0db8fdcf026f 44 motor.Drive(1, CW, 2000);
12f2027a 0:0db8fdcf026f 45 }
12f2027a 0:0db8fdcf026f 46 /*白＿黒＿白＿白＿白*/
12f2027a 0:0db8fdcf026f 47 else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:0db8fdcf026f 48 motor.Drive(0, CW, 1500);
12f2027a 0:0db8fdcf026f 49 motor.Drive(1, CW, 2000);
12f2027a 0:0db8fdcf026f 50 }
12f2027a 0:0db8fdcf026f 51 /*白＿黒＿黒＿白＿白*/
12f2027a 0:0db8fdcf026f 52 else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:0db8fdcf026f 53 motor.Drive(0, CW, 1800);
12f2027a 0:0db8fdcf026f 54 motor.Drive(1, CW, 2000);
12f2027a 0:0db8fdcf026f 55 }
12f2027a 0:0db8fdcf026f 56 /*白＿白＿黒＿白＿白*/
12f2027a 0:0db8fdcf026f 57 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:0db8fdcf026f 58 motor.Drive(0, CW, 2000);
12f2027a 0:0db8fdcf026f 59 motor.Drive(1, CW, 2000);
12f2027a 0:0db8fdcf026f 60 }
12f2027a 0:0db8fdcf026f 61
12f2027a 0:0db8fdcf026f 62 /*白＿白＿黒＿黒＿白*/
12f2027a 0:0db8fdcf026f 63 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
12f2027a 0:0db8fdcf026f 64 motor.Drive(0, CW, 2000);
12f2027a 0:0db8fdcf026f 65 motor.Drive(1, CW, 1800);
12f2027a 0:0db8fdcf026f 66 }
12f2027a 0:0db8fdcf026f 67 /*白＿白＿白＿黒＿白*/
12f2027a 0:0db8fdcf026f 68 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
12f2027a 0:0db8fdcf026f 69 motor.Drive(0, CW, 2000);
12f2027a 0:0db8fdcf026f 70 motor.Drive(1, CW, 1500);
12f2027a 0:0db8fdcf026f 71 }
12f2027a 0:0db8fdcf026f 72 /*白＿白＿白＿白＿黒*/
12f2027a 0:0db8fdcf026f 73 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
12f2027a 0:0db8fdcf026f 74 motor.Drive(0, CW, 2000);
12f2027a 0:0db8fdcf026f 75 motor.Drive(1, CW, 500);
12f2027a 0:0db8fdcf026f 76 }
12f2027a 0:0db8fdcf026f 77
12f2027a 0:0db8fdcf026f 78 ///////////大きい右カーブを曲がるためのプログラム//////////
12f2027a 0:0db8fdcf026f 79 /*1センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 80 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
12f2027a 0:0db8fdcf026f 81 AI = 1;
12f2027a 0:0db8fdcf026f 82 }
12f2027a 0:0db8fdcf026f 83
12f2027a 0:0db8fdcf026f 84 /*2センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 85 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:0db8fdcf026f 86 AI = 1;
12f2027a 0:0db8fdcf026f 87 }
12f2027a 0:0db8fdcf026f 88 /*3センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 89 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:0db8fdcf026f 90 AI = 1;
12f2027a 0:0db8fdcf026f 91 }
12f2027a 0:0db8fdcf026f 92
12f2027a 0:0db8fdcf026f 93 /*4センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 94 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:0db8fdcf026f 95 AI = 1;
12f2027a 0:0db8fdcf026f 96 }
12f2027a 0:0db8fdcf026f 97
12f2027a 0:0db8fdcf026f 98 /*5センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 99 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:0db8fdcf026f 100 AI = 1;
12f2027a 0:0db8fdcf026f 101 }
12f2027a 0:0db8fdcf026f 102
12f2027a 0:0db8fdcf026f 103 ///////////大きい左カーブを曲がるためのプログラム//////////
12f2027a 0:0db8fdcf026f 104 /*6センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 105 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 106 AI = 2;
12f2027a 0:0db8fdcf026f 107 }
12f2027a 0:0db8fdcf026f 108
12f2027a 0:0db8fdcf026f 109 /*7センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 110 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 111 AI = 2;
12f2027a 0:0db8fdcf026f 112 }
12f2027a 0:0db8fdcf026f 113
12f2027a 0:0db8fdcf026f 114 /*8センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 115 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 116 AI = 2;
12f2027a 0:0db8fdcf026f 117 }
12f2027a 0:0db8fdcf026f 118
12f2027a 0:0db8fdcf026f 119 /*9センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 120 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 121 AI = 2;
12f2027a 0:0db8fdcf026f 122 }
12f2027a 0:0db8fdcf026f 123
12f2027a 0:0db8fdcf026f 124 /*10センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:0db8fdcf026f 125 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 126 AI = 2;
12f2027a 0:0db8fdcf026f 127 }
12f2027a 0:0db8fdcf026f 128
12f2027a 0:0db8fdcf026f 129
12f2027a 0:0db8fdcf026f 130
12f2027a 0:0db8fdcf026f 131 /*11全部白かつAI = 1*/
12f2027a 0:0db8fdcf026f 132 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 133 motor.Drive(0, CW, 1200);
12f2027a 0:0db8fdcf026f 134 motor.Drive(1, CCW, 1200);
12f2027a 0:0db8fdcf026f 135 }
12f2027a 0:0db8fdcf026f 136 /*12全部白かつAI = 2*/
12f2027a 0:0db8fdcf026f 137 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:0db8fdcf026f 138 motor.Drive(0, CCW, 1200);
12f2027a 0:0db8fdcf026f 139 motor.Drive(1, CW, 1200);
12f2027a 0:0db8fdcf026f 140 }
12f2027a 0:0db8fdcf026f 141
12f2027a 0:0db8fdcf026f 142 /**********************************/
12f2027a 0:0db8fdcf026f 143 /*停止位置で止まるためのプログラム*/
12f2027a 0:0db8fdcf026f 144 /**********************************/
12f2027a 0:0db8fdcf026f 145
12f2027a 0:0db8fdcf026f 146 /*1.4.1 センサ1-5が黒*/
12f2027a 0:0db8fdcf026f 147 else if(sw1 <= 0.5 && sw5 <= 0.5){
12f2027a 0:0db8fdcf026f 148
12f2027a 0:0db8fdcf026f 149
12f2027a 0:0db8fdcf026f 150
12f2027a 0:0db8fdcf026f 151 if( updown == 0 ){
12f2027a 0:0db8fdcf026f 152 ///////////停止//////////
12f2027a 0:0db8fdcf026f 153 motor.Drive(0, STOP, 0);
12f2027a 0:0db8fdcf026f 154 motor.Drive(1, STOP, 0);
12f2027a 0:0db8fdcf026f 155 wait(0.1);
12f2027a 0:0db8fdcf026f 156
12f2027a 0:0db8fdcf026f 157 ///////////リフト上げる//////////
12f2027a 0:0db8fdcf026f 158 lift.Drive(0, CCW, 2000);
12f2027a 0:0db8fdcf026f 159 wait(1.5);
12f2027a 0:0db8fdcf026f 160 ///////////リフト停止//////////
12f2027a 0:0db8fdcf026f 161 lift.Drive(0, STOP, 0);
12f2027a 0:0db8fdcf026f 162 wait(0.1);
12f2027a 0:0db8fdcf026f 163 /*
12f2027a 0:0db8fdcf026f 164 ///////////停止位置を超えるために少し前進//////////
12f2027a 0:0db8fdcf026f 165
12f2027a 0:0db8fdcf026f 166 motor.Drive(0, CW, 1200);
12f2027a 0:0db8fdcf026f 167 motor.Drive(1, CW, 1200);
12f2027a 0:0db8fdcf026f 168 wait(0.1);
12f2027a 0:0db8fdcf026f 169 */
12f2027a 0:0db8fdcf026f 170 updown = 1;
12f2027a 0:0db8fdcf026f 171 }
12f2027a 0:0db8fdcf026f 172
12f2027a 0:0db8fdcf026f 173 else if( updown == 1 ){
12f2027a 0:0db8fdcf026f 174 ///////////停止//////////
12f2027a 0:0db8fdcf026f 175 motor.Drive(0, STOP, 0);
12f2027a 0:0db8fdcf026f 176 motor.Drive(1, STOP, 0);
12f2027a 0:0db8fdcf026f 177 wait(0.1);
12f2027a 0:0db8fdcf026f 178 ///////////リフト下げる//////////
12f2027a 0:0db8fdcf026f 179 lift.Drive(0, CW, 4095);
12f2027a 0:0db8fdcf026f 180 wait(1.0);
12f2027a 0:0db8fdcf026f 181 ///////////リフト停止//////////
12f2027a 0:0db8fdcf026f 182 lift.Drive(0, STOP, 0);
12f2027a 0:0db8fdcf026f 183 wait(0.1);
12f2027a 0:0db8fdcf026f 184 ///////////バック//////////
12f2027a 0:0db8fdcf026f 185 motor.Drive(0, CCW, 4095);
12f2027a 0:0db8fdcf026f 186 motor.Drive(1, CCW, 4095);
12f2027a 0:0db8fdcf026f 187 wait(1.2);
12f2027a 0:0db8fdcf026f 188 ///////////モータ停止//////////
12f2027a 0:0db8fdcf026f 189 motor.Drive(0, STOP, 0);
12f2027a 0:0db8fdcf026f 190 motor.Drive(1, STOP, 0);
12f2027a 0:0db8fdcf026f 191 wait(1.0);
12f2027a 0:0db8fdcf026f 192 updown = 0;
12f2027a 0:0db8fdcf026f 193 break;
12f2027a 0:0db8fdcf026f 194 }
12f2027a 0:0db8fdcf026f 195 }
12f2027a 0:0db8fdcf026f 196
12f2027a 0:0db8fdcf026f 197
12f2027a 0:0db8fdcf026f 198
12f2027a 0:0db8fdcf026f 199
12f2027a 0:0db8fdcf026f 200
12f2027a 0:0db8fdcf026f 201 }
12f2027a 0:0db8fdcf026f 202 }