shinnosuke hanamura / Mbed 2 deprecated f

a

Dependencies:   MotorDriver_SU mbed

c.cpp@0:a421b8dde3ab, 2014-12-04 (annotated)

Committer:
hanashin
Date:
Thu Dec 04 06:49:04 2014 +0000
Revision:
0:a421b8dde3ab
1204

Who changed what in which revision?

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