Naoki Hasegawa / Mbed 2 deprecated finalcenter1124

six sensors

Dependencies:   MotorDriver_SU mbed

Fork of finalcenter by masayushi nisino

center.cpp@2:744aaae91c45, 2014-11-24 (annotated)

Committer:
12F2041G
Date:
Mon Nov 24 06:48:35 2014 +0000
Revision:
2:744aaae91c45
Parent:
1:0cd50a576350 
1124

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 2:744aaae91c45 11 DigitalIn sensor6(Px_xx);
12F2041G 0:cc0c03ee56b3 12 DigitalIn sensor5(P0_22);
12F2041G 2:744aaae91c45 13 DigitalIn sensor7(Px_xx);
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
12F2041G 0:cc0c03ee56b3 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){
12f2039e 1:0cd50a576350 41 motor.Drive(0, CW, 1700);
12f2039e 1:0cd50a576350 42 motor.Drive(1, CW, 1700);
12F2041G 0:cc0c03ee56b3 43 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 44 }
12F2041G 2:744aaae91c45 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)){
12f2039e 1:0cd50a576350 46 led = 1;//count kakuninn
12F2041G 0:cc0c03ee56b3 47 if( count == 0){
12F2041G 0:cc0c03ee56b3 48 lift.Drive(0, CW, 4095); //lift up
12F2041G 0:cc0c03ee56b3 49 motor.Drive(0, STOP, 0); //stop
12F2041G 0:cc0c03ee56b3 50 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 51 wait(1);
12f2039e 1:0cd50a576350 52 led = 0;//count kakuninn
12f2039e 1:0cd50a576350 53 motor.Drive(0, CW, 2000); //restart
12f2039e 1:0cd50a576350 54 motor.Drive(1, CW, 2000);
12F2041G 0:cc0c03ee56b3 55 lift.Drive(0, CW, 4095); //lift up more
12F2041G 0:cc0c03ee56b3 56 wait(0.7);
12F2041G 0:cc0c03ee56b3 57 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 58 //height = 1;//lift position high
12F2041G 0:cc0c03ee56b3 59 count++;
12F2041G 0:cc0c03ee56b3 60 }
12F2041G 0:cc0c03ee56b3 61 //need to change count[1,2,3...] after test
12F2041G 0:cc0c03ee56b3 62 else if(count == 1){
12F2041G 0:cc0c03ee56b3 63 count++;
12F2041G 0:cc0c03ee56b3 64 }
12F2041G 0:cc0c03ee56b3 65 else if(count == 2){
12F2041G 0:cc0c03ee56b3 66 count++;
12F2041G 0:cc0c03ee56b3 67 }
12F2041G 0:cc0c03ee56b3 68 else if(count == 3){
12F2041G 0:cc0c03ee56b3 69 motor.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 70 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 71 lift.Drive(0, CCW, 2000);//lift down
12F2041G 0:cc0c03ee56b3 72 wait(1.3);
12F2041G 0:cc0c03ee56b3 73 motor.Drive(0, CCW, 4095);//back for 2 seconds
12F2041G 0:cc0c03ee56b3 74 motor.Drive(1, CCW, 4095);
12F2041G 0:cc0c03ee56b3 75 lift.Drive(0, STOP, 0);
12f2039e 1:0cd50a576350 76 led = 0;//count kakunin
12F2041G 0:cc0c03ee56b3 77 wait(2);
12F2041G 0:cc0c03ee56b3 78 //height = 0;//lift position low
12F2041G 0:cc0c03ee56b3 79 count = 0;//reset
12F2041G 0:cc0c03ee56b3 80 }
12F2041G 0:cc0c03ee56b3 81 else{
12F2041G 0:cc0c03ee56b3 82 ;
12F2041G 0:cc0c03ee56b3 83 }
12F2041G 0:cc0c03ee56b3 84 }
12F2041G 0:cc0c03ee56b3 85 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 86 motor.Drive(0, CW, 1500);
12f2039e 1:0cd50a576350 87 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 88 }
12F2041G 0:cc0c03ee56b3 89 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 90 motor.Drive(0, CW, 1500);
12F2041G 0:cc0c03ee56b3 91 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 92 }
12F2041G 0:cc0c03ee56b3 93 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
12F2041G 0:cc0c03ee56b3 94 motor.Drive(0, CW, 500);
12F2041G 0:cc0c03ee56b3 95 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 96 }
12F2041G 0:cc0c03ee56b3 97 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
12f2039e 1:0cd50a576350 98 motor.Drive(0, CW, 500);//ccw change cw
12f2039e 1:0cd50a576350 99 motor.Drive(1, CW, 500);//ccw change cw
12F2041G 0:cc0c03ee56b3 100 }
12F2041G 0:cc0c03ee56b3 101 /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
12F2041G 0:cc0c03ee56b3 102 motor.Drive(0, CW, 2048);
12F2041G 0:cc0c03ee56b3 103 motor.Drive(1, STOP, 0);
12F2041G 0:cc0c03ee56b3 104 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 105 }*/
12F2041G 0:cc0c03ee56b3 106 //all body on line's outside
12F2041G 0:cc0c03ee56b3 107 else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
12F2041G 0:cc0c03ee56b3 108 motor.Drive(0, CCW, 200);
12F2041G 0:cc0c03ee56b3 109 motor.Drive(1, CW, 2000);
12F2041G 0:cc0c03ee56b3 110 lift.Drive(0, STOP, 0);
12F2041G 0:cc0c03ee56b3 111 }
12F2041G 0:cc0c03ee56b3 112 //select course condition
12F2041G 2:744aaae91c45 113 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 114 if(check1 == 0){
12F2041G 0:cc0c03ee56b3 115 check1 = 1;
12f2039e 1:0cd50a576350 116 motor.Drive(0, CW, 1000);//adjust
12f2039e 1:0cd50a576350 117 motor.Drive(1, CW, 1000);
12f2039e 1:0cd50a576350 118 wait(0.5);//adjust
12F2041G 0:cc0c03ee56b3 119 //enter the right courses
12F2041G 0:cc0c03ee56b3 120 motor.Drive(0, CW, 500);//change both speed
12F2041G 0:cc0c03ee56b3 121 motor.Drive(1, CW, 1500);
12F2041G 0:cc0c03ee56b3 122 wait(1);
12F2041G 0:cc0c03ee56b3 123 }
12F2041G 0:cc0c03ee56b3 124 else{
12F2041G 0:cc0c03ee56b3 125 ;//response only once
12F2041G 0:cc0c03ee56b3 126 }
12F2041G 0:cc0c03ee56b3 127 }
12F2041G 2:744aaae91c45 128 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 129 if(check2 == 0){
12F2041G 0:cc0c03ee56b3 130 check2 = 1;
12F2041G 0:cc0c03ee56b3 131 //enter the center course
12F2041G 0:cc0c03ee56b3 132 motor.Drive(0, CW, 1500);//change both speed
12F2041G 0:cc0c03ee56b3 133 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 134 wait(1);
12F2041G 0:cc0c03ee56b3 135 }
12f2039e 1:0cd50a576350 136 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0 ) {
12f2039e 1:0cd50a576350 137 motor.Drive(0, CW, 500);
12f2039e 1:0cd50a576350 138 motor.Drive(1, CW, 500);
12f2039e 1:0cd50a576350 139 }//tuika
12f2039e 1:0cd50a576350 140
12f2039e 1:0cd50a576350 141
12F2041G 0:cc0c03ee56b3 142 else{
12F2041G 0:cc0c03ee56b3 143 ;//response only once
12F2041G 0:cc0c03ee56b3 144 }
12F2041G 0:cc0c03ee56b3 145 }
12f2039e 1:0cd50a576350 146
12F2041G 0:cc0c03ee56b3 147 else{
12F2041G 0:cc0c03ee56b3 148 motor.Drive(0, CW, 2500);//spin to search correct way
12F2041G 0:cc0c03ee56b3 149 motor.Drive(1, CW, 500);
12F2041G 0:cc0c03ee56b3 150 }
12F2041G 0:cc0c03ee56b3 151 }
12F2041G 0:cc0c03ee56b3 152 }
12F2041G 0:cc0c03ee56b3 153 }