shinnosuke hanamura / Mbed 2 deprecated finalcenter1

1

Dependencies:   MotorDriver_SU mbed

Fork of finalcenter by Naoki Hasegawa

center.cpp@1:cd2d0caaa312, 2014-12-13 (annotated)

Committer:
hanashin
Date:
Sat Dec 13 03:12:11 2014 +0000
Revision:
1:cd2d0caaa312
Parent:
0:cc0c03ee56b3 
2

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12F2041G 0:cc0c03ee56b3 1 #include "mbed.h"
12F2041G 0:cc0c03ee56b3 2 #include "MotorDriver_SU.h"
12F2041G 0:cc0c03ee56b3 3
12F2041G 0:cc0c03ee56b3 4 DigitalIn sw(P0_1, PullUp);
12F2041G 0:cc0c03ee56b3 5 // trace L to R
12F2041G 0:cc0c03ee56b3 6 DigitalIn sensor1(P0_16);
12F2041G 0:cc0c03ee56b3 7 DigitalIn sensor2(P0_13);
12F2041G 0:cc0c03ee56b3 8 DigitalIn sensor3(P0_11);
12F2041G 0:cc0c03ee56b3 9 // lift L to R
12F2041G 0:cc0c03ee56b3 10 DigitalIn sensor4(P0_21);
hanashin 1:cd2d0caaa312 11 DigitalIn sensor6(P0_14);
12F2041G 0:cc0c03ee56b3 12 DigitalIn sensor5(P0_22);
hanashin 1:cd2d0caaa312 13 DigitalIn sensor7(P0_23);
12F2041G 0:cc0c03ee56b3 14
12F2041G 0:cc0c03ee56b3 15 DigitalOut led(P1_28);
12F2041G 0:cc0c03ee56b3 16
12F2041G 0:cc0c03ee56b3 17 MotorDriver_SU motor(MOTOR_DOUBLE);
12F2041G 0:cc0c03ee56b3 18 MotorDriver_SU lift(MOTOR_SINGLE);
12F2041G 0:cc0c03ee56b3 19
12F2041G 0:cc0c03ee56b3 20 int main(){
12F2041G 0:cc0c03ee56b3 21 while(1){
12F2041G 0:cc0c03ee56b3 22
12F2041G 0:cc0c03ee56b3 23 //int height = 0;//lift position
12F2041G 0:cc0c03ee56b3 24 int check1 = 0;//convert count
hanashin 1:cd2d0caaa312 25 /*int check2 = 0;*/
12F2041G 0:cc0c03ee56b3 26 int count = 0;//count liftsensor B&B
12F2041G 0:cc0c03ee56b3 27 while(sw == 0){
12F2041G 0:cc0c03ee56b3 28 motor.Drive(0, STOP, 0); //R
12F2041G 0:cc0c03ee56b3 29 motor.Drive(1, STOP, 0);//L
12F2041G 0:cc0c03ee56b3 30 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 31 }
12F2041G 0:cc0c03ee56b3 32 //////////////////////////////////////////////////////////
12F2041G 0:cc0c03ee56b3 33 //need to change some numerical value after test
12F2041G 0:cc0c03ee56b3 34 //////////////////////////////////////////////////////////
12F2041G 0:cc0c03ee56b3 35 while(sw == 1){
12F2041G 0:cc0c03ee56b3 36 if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
12F2041G 0:cc0c03ee56b3 37 motor.Drive(0, CW, 500);
12F2041G 0:cc0c03ee56b3 38 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 39 }
12F2041G 0:cc0c03ee56b3 40 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
hanashin 1:cd2d0caaa312 41 motor.Drive(0, CW, 2000);
hanashin 1:cd2d0caaa312 42 motor.Drive(1, CW, 2000);
12F2041G 0:cc0c03ee56b3 43 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 44 }
hanashin 1:cd2d0caaa312 45 else if((sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0)/*||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor7 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor6 == 0 && sensor5 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor5 == 0 && sensor7 == 0)*/){
hanashin 1:cd2d0caaa312 46 led = 1;//count kakuninn
12F2041G 0:cc0c03ee56b3 47 if( count == 0){
12F2041G 0:cc0c03ee56b3 48 lift.Drive(0, CW, 4095); //lift up
hanashin 1:cd2d0caaa312 49 motor.Drive(0, CW, 1300); //restart
hanashin 1:cd2d0caaa312 50 motor.Drive(1, CW, 1300);
hanashin 1:cd2d0caaa312 51 /*motor.Drive(0, STOP, 0); //stop
hanashin 1:cd2d0caaa312 52 motor.Drive(1, STOP, 0);*/
hanashin 1:cd2d0caaa312 53 wait(1.8);
hanashin 1:cd2d0caaa312 54 led = 0;//count kakuninn
hanashin 1:cd2d0caaa312 55 /* motor.Drive(0, CW, 2000); //restart
hanashin 1:cd2d0caaa312 56 motor.Drive(1, CW, 2000);
12F2041G 0:cc0c03ee56b3 57 lift.Drive(0, CW, 4095); //lift up more
hanashin 1:cd2d0caaa312 58 wait(0.7);*/
12F2041G 0:cc0c03ee56b3 59 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 60 //height = 1;//lift position high
12F2041G 0:cc0c03ee56b3 61 count++;
12F2041G 0:cc0c03ee56b3 62 }
12F2041G 0:cc0c03ee56b3 63 //need to change count[1,2,3...] after test
12F2041G 0:cc0c03ee56b3 64 else if(count == 1){
hanashin 1:cd2d0caaa312 65 count = 3;
12F2041G 0:cc0c03ee56b3 66 }
12F2041G 0:cc0c03ee56b3 67 else if(count == 3){
12F2041G 0:cc0c03ee56b3 68 motor.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 69 motor.Drive(1, STOP, 0);
hanashin 1:cd2d0caaa312 70 lift.Drive(0, CCW, 3000);//lift down
12F2041G 0:cc0c03ee56b3 71 wait(1.3);
12F2041G 0:cc0c03ee56b3 72 motor.Drive(0, CCW, 4095);//back for 2 seconds
12F2041G 0:cc0c03ee56b3 73 motor.Drive(1, CCW, 4095);
12F2041G 0:cc0c03ee56b3 74 lift.Drive(0, STOP, 0);
hanashin 1:cd2d0caaa312 75 led = 0;//count kakunin
12F2041G 0:cc0c03ee56b3 76 wait(2);
12F2041G 0:cc0c03ee56b3 77 //height = 0;//lift position low
12F2041G 0:cc0c03ee56b3 78 count = 0;//reset
12F2041G 0:cc0c03ee56b3 79 }
12F2041G 0:cc0c03ee56b3 80 else{
12F2041G 0:cc0c03ee56b3 81 ;
12F2041G 0:cc0c03ee56b3 82 }
12F2041G 0:cc0c03ee56b3 83 }
12F2041G 0:cc0c03ee56b3 84 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 85 motor.Drive(0, CW, 1500);
hanashin 1:cd2d0caaa312 86 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 87 }
12F2041G 0:cc0c03ee56b3 88 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 89 motor.Drive(0, CW, 1500);
hanashin 1:cd2d0caaa312 90 motor.Drive(1, CW, 1200);
12F2041G 0:cc0c03ee56b3 91 }
12F2041G 0:cc0c03ee56b3 92 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
hanashin 1:cd2d0caaa312 93 motor.Drive(0, CW, 1200);
12F2041G 0:cc0c03ee56b3 94 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 95 }
12F2041G 0:cc0c03ee56b3 96 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
hanashin 1:cd2d0caaa312 97 motor.Drive(0, CW, 500);//ccw change cw
hanashin 1:cd2d0caaa312 98 motor.Drive(1, CW, 500);//ccw change cw
12F2041G 0:cc0c03ee56b3 99 }
12F2041G 0:cc0c03ee56b3 100 /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 101 motor.Drive(0, CW, 2048);
12F2041G 0:cc0c03ee56b3 102 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 103 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 104 }*/
12F2041G 0:cc0c03ee56b3 105 //all body on line's outside
12F2041G 0:cc0c03ee56b3 106 else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
12F2041G 0:cc0c03ee56b3 107 motor.Drive(0, CCW, 200);
12F2041G 0:cc0c03ee56b3 108 motor.Drive(1, CW, 2000);
12F2041G 0:cc0c03ee56b3 109 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 110 }
12F2041G 0:cc0c03ee56b3 111 //select course condition
hanashin 1:cd2d0caaa312 112 else if((sensor2 == 0 && sensor4 == 1 && sensor5 == 0 )||(sensor2 == 0 && sensor4 == 1 && sensor7 == 0 )||(sensor2 == 0 && sensor6 == 1 && sensor5 == 0)||(sensor2 == 0 && sensor6 == 1 && sensor7 == 0)){
12F2041G 0:cc0c03ee56b3 113 if(check1 == 0){
12F2041G 0:cc0c03ee56b3 114 check1 = 1;
hanashin 1:cd2d0caaa312 115 motor.Drive(0, CW, 1500);//adjust
hanashin 1:cd2d0caaa312 116 motor.Drive(1, CW, 1500);
hanashin 1:cd2d0caaa312 117 wait(0.5);//adjust
12F2041G 0:cc0c03ee56b3 118 //enter the right courses
12F2041G 0:cc0c03ee56b3 119 motor.Drive(0, CW, 500);//change both speed
hanashin 1:cd2d0caaa312 120 motor.Drive(1, CW, 2500);
hanashin 1:cd2d0caaa312 121 wait(1);
hanashin 1:cd2d0caaa312 122 motor.Drive(0, CW, 1500);//adjust
12F2041G 0:cc0c03ee56b3 123 motor.Drive(1, CW, 1500);
hanashin 1:cd2d0caaa312 124 wait(0.15);//adjust
hanashin 1:cd2d0caaa312 125 motor.Drive(0, CW, 1200);//change both speed
hanashin 1:cd2d0caaa312 126 motor.Drive(1, CCW, 200);
hanashin 1:cd2d0caaa312 127 wait(0.6);
hanashin 1:cd2d0caaa312 128 /*motor.Drive(0, CW, 1500);//adjust
hanashin 1:cd2d0caaa312 129 motor.Drive(1, CW, 1500);
hanashin 1:cd2d0caaa312 130 wait(0.1);//adjust*/
12F2041G 0:cc0c03ee56b3 131 }
12F2041G 0:cc0c03ee56b3 132 else{
12F2041G 0:cc0c03ee56b3 133 ;//response only once
12F2041G 0:cc0c03ee56b3 134 }
12F2041G 0:cc0c03ee56b3 135 }
hanashin 1:cd2d0caaa312 136 /*else if((sensor2 == 0 && sensor4 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor4 == 0 && sensor7 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor7 == 1)){
12F2041G 0:cc0c03ee56b3 137 if(check2 == 0){
12F2041G 0:cc0c03ee56b3 138 check2 = 1;
12F2041G 0:cc0c03ee56b3 139 //enter the center course
hanashin 1:cd2d0caaa312 140 motor.Drive(0, CW, 800);//change both speed
hanashin 1:cd2d0caaa312 141 motor.Drive(1, CCW, 500);
12F2041G 0:cc0c03ee56b3 142 wait(1);
12F2041G 0:cc0c03ee56b3 143 }
hanashin 1:cd2d0caaa312 144 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0 ) {
hanashin 1:cd2d0caaa312 145 motor.Drive(0, CW, 500);
hanashin 1:cd2d0caaa312 146 motor.Drive(1, CW, 500);
hanashin 1:cd2d0caaa312 147 }//tuika
hanashin 1:cd2d0caaa312 148
hanashin 1:cd2d0caaa312 149
12F2041G 0:cc0c03ee56b3 150 else{
12F2041G 0:cc0c03ee56b3 151 ;//response only once
12F2041G 0:cc0c03ee56b3 152 }
12F2041G 0:cc0c03ee56b3 153 }
hanashin 1:cd2d0caaa312 154 */
12F2041G 0:cc0c03ee56b3 155 else{
12F2041G 0:cc0c03ee56b3 156 motor.Drive(0, CW, 2500);//spin to search correct way
12F2041G 0:cc0c03ee56b3 157 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 158 }
12F2041G 0:cc0c03ee56b3 159 }
12F2041G 0:cc0c03ee56b3 160 }
12F2041G 0:cc0c03ee56b3 161 }