shinnosuke hanamura / Mbed 2 deprecated finalcenter1

1

Dependencies:   MotorDriver_SU mbed

Fork of finalcenter by Naoki Hasegawa

center.cpp@0:cc0c03ee56b3, 2014-11-19 (annotated)

Committer:
12F2041G
Date:
Wed Nov 19 13:08:25 2014 +0000
Revision:
0:cc0c03ee56b3
Child:
1:cd2d0caaa312
11/19 final(center)

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);
12F2041G 0:cc0c03ee56b3 11 DigitalIn sensor5(P0_22);
12F2041G 0:cc0c03ee56b3 12
12F2041G 0:cc0c03ee56b3 13 DigitalOut led(P1_28);
12F2041G 0:cc0c03ee56b3 14
12F2041G 0:cc0c03ee56b3 15 MotorDriver_SU motor(MOTOR_DOUBLE);
12F2041G 0:cc0c03ee56b3 16 MotorDriver_SU lift(MOTOR_SINGLE);
12F2041G 0:cc0c03ee56b3 17
12F2041G 0:cc0c03ee56b3 18 int main(){
12F2041G 0:cc0c03ee56b3 19 while(1){
12F2041G 0:cc0c03ee56b3 20
12F2041G 0:cc0c03ee56b3 21 //int height = 0;//lift position
12F2041G 0:cc0c03ee56b3 22 int check1 = 0;//convert count
12F2041G 0:cc0c03ee56b3 23 int check2 = 0;
12F2041G 0:cc0c03ee56b3 24 int count = 0;//count liftsensor B&B
12F2041G 0:cc0c03ee56b3 25 while(sw == 0){
12F2041G 0:cc0c03ee56b3 26 motor.Drive(0, STOP, 0); //R
12F2041G 0:cc0c03ee56b3 27 motor.Drive(1, STOP, 0);//L
12F2041G 0:cc0c03ee56b3 28 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 29 }
12F2041G 0:cc0c03ee56b3 30 //////////////////////////////////////////////////////////
12F2041G 0:cc0c03ee56b3 31 //need to change some numerical value after test
12F2041G 0:cc0c03ee56b3 32 //////////////////////////////////////////////////////////
12F2041G 0:cc0c03ee56b3 33 while(sw == 1){
12F2041G 0:cc0c03ee56b3 34 if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
12F2041G 0:cc0c03ee56b3 35 motor.Drive(0, CW, 500);
12F2041G 0:cc0c03ee56b3 36 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 37 }
12F2041G 0:cc0c03ee56b3 38 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
12F2041G 0:cc0c03ee56b3 39 motor.Drive(0, CW, 1000);
12F2041G 0:cc0c03ee56b3 40 motor.Drive(1, CW, 1000);
12F2041G 0:cc0c03ee56b3 41 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 42 }
12F2041G 0:cc0c03ee56b3 43 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){
12F2041G 0:cc0c03ee56b3 44 if( count == 0){
12F2041G 0:cc0c03ee56b3 45 lift.Drive(0, CW, 4095); //lift up
12F2041G 0:cc0c03ee56b3 46 motor.Drive(0, STOP, 0); //stop
12F2041G 0:cc0c03ee56b3 47 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 48 wait(1);
12F2041G 0:cc0c03ee56b3 49 motor.Drive(0, CW, 1000); //restart
12F2041G 0:cc0c03ee56b3 50 motor.Drive(1, CW, 1000);
12F2041G 0:cc0c03ee56b3 51 lift.Drive(0, CW, 4095); //lift up more
12F2041G 0:cc0c03ee56b3 52 wait(0.7);
12F2041G 0:cc0c03ee56b3 53 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 54 //height = 1;//lift position high
12F2041G 0:cc0c03ee56b3 55 count++;
12F2041G 0:cc0c03ee56b3 56 }
12F2041G 0:cc0c03ee56b3 57 //need to change count[1,2,3...] after test
12F2041G 0:cc0c03ee56b3 58 else if(count == 1){
12F2041G 0:cc0c03ee56b3 59 count++;
12F2041G 0:cc0c03ee56b3 60 }
12F2041G 0:cc0c03ee56b3 61 else if(count == 2){
12F2041G 0:cc0c03ee56b3 62 count++;
12F2041G 0:cc0c03ee56b3 63 }
12F2041G 0:cc0c03ee56b3 64 else if(count == 3){
12F2041G 0:cc0c03ee56b3 65 motor.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 66 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 67 lift.Drive(0, CCW, 2000);//lift down
12F2041G 0:cc0c03ee56b3 68 wait(1.3);
12F2041G 0:cc0c03ee56b3 69 motor.Drive(0, CCW, 4095);//back for 2 seconds
12F2041G 0:cc0c03ee56b3 70 motor.Drive(1, CCW, 4095);
12F2041G 0:cc0c03ee56b3 71 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 72 wait(2);
12F2041G 0:cc0c03ee56b3 73 //height = 0;//lift position low
12F2041G 0:cc0c03ee56b3 74 count = 0;//reset
12F2041G 0:cc0c03ee56b3 75 }
12F2041G 0:cc0c03ee56b3 76 else{
12F2041G 0:cc0c03ee56b3 77 ;
12F2041G 0:cc0c03ee56b3 78 }
12F2041G 0:cc0c03ee56b3 79 }
12F2041G 0:cc0c03ee56b3 80 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 81 motor.Drive(0, CW, 1500);
12F2041G 0:cc0c03ee56b3 82 motor.Drive(1, STOP, 500);
12F2041G 0:cc0c03ee56b3 83 }
12F2041G 0:cc0c03ee56b3 84 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 85 motor.Drive(0, CW, 1500);
12F2041G 0:cc0c03ee56b3 86 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 87 }
12F2041G 0:cc0c03ee56b3 88 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
12F2041G 0:cc0c03ee56b3 89 motor.Drive(0, CW, 500);
12F2041G 0:cc0c03ee56b3 90 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 91 }
12F2041G 0:cc0c03ee56b3 92 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
12F2041G 0:cc0c03ee56b3 93 motor.Drive(0, CCW, 500);
12F2041G 0:cc0c03ee56b3 94 motor.Drive(1, CCW, 500);
12F2041G 0:cc0c03ee56b3 95 }
12F2041G 0:cc0c03ee56b3 96 /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 97 motor.Drive(0, CW, 2048);
12F2041G 0:cc0c03ee56b3 98 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 99 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 100 }*/
12F2041G 0:cc0c03ee56b3 101 //all body on line's outside
12F2041G 0:cc0c03ee56b3 102 else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
12F2041G 0:cc0c03ee56b3 103 motor.Drive(0, CCW, 200);
12F2041G 0:cc0c03ee56b3 104 motor.Drive(1, CW, 2000);
12F2041G 0:cc0c03ee56b3 105 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 106 }
12F2041G 0:cc0c03ee56b3 107 //select course condition
12F2041G 0:cc0c03ee56b3 108 else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){
12F2041G 0:cc0c03ee56b3 109 if(check1 == 0){
12F2041G 0:cc0c03ee56b3 110 check1 = 1;
12F2041G 0:cc0c03ee56b3 111 //enter the right courses
12F2041G 0:cc0c03ee56b3 112 motor.Drive(0, CW, 500);//change both speed
12F2041G 0:cc0c03ee56b3 113 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 114 wait(1);
12F2041G 0:cc0c03ee56b3 115 }
12F2041G 0:cc0c03ee56b3 116 else{
12F2041G 0:cc0c03ee56b3 117 ;//response only once
12F2041G 0:cc0c03ee56b3 118 }
12F2041G 0:cc0c03ee56b3 119 }
12F2041G 0:cc0c03ee56b3 120 else if(sensor2 == 0 && sensor4 == 0 && sensor5 == 1){
12F2041G 0:cc0c03ee56b3 121 if(check2 == 0){
12F2041G 0:cc0c03ee56b3 122 check2 = 1;
12F2041G 0:cc0c03ee56b3 123 //enter the center course
12F2041G 0:cc0c03ee56b3 124 motor.Drive(0, CW, 1500);//change both speed
12F2041G 0:cc0c03ee56b3 125 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 126 wait(1);
12F2041G 0:cc0c03ee56b3 127 }
12F2041G 0:cc0c03ee56b3 128 else{
12F2041G 0:cc0c03ee56b3 129 ;//response only once
12F2041G 0:cc0c03ee56b3 130 }
12F2041G 0:cc0c03ee56b3 131 }
12F2041G 0:cc0c03ee56b3 132 else{
12F2041G 0:cc0c03ee56b3 133 motor.Drive(0, CW, 2500);//spin to search correct way
12F2041G 0:cc0c03ee56b3 134 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 135 }
12F2041G 0:cc0c03ee56b3 136 }
12F2041G 0:cc0c03ee56b3 137 }
12F2041G 0:cc0c03ee56b3 138 }