masayushi nisino / Mbed 2 deprecated finalcenter

last

Dependencies:   MotorDriver_SU mbed

Fork of finalcenter by Naoki Hasegawa

center.cpp@1:0cd50a576350, 2014-11-23 (annotated)

Committer:
12f2039e
Date:
Sun Nov 23 09:08:39 2014 +0000
Revision:
1:0cd50a576350
Parent:
0:cc0c03ee56b3 
last

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