Naoki Hasegawa
a
Dependencies: MotorDriver_SU mbed
Revision 0:cc0c03ee56b3, committed 2014-11-19
- Comitter:
- 12F2041G
- Date:
- Wed Nov 19 13:08:25 2014 +0000
- Commit message:
- 11/19 final(center)
Changed in this revision
diff -r 000000000000 -r cc0c03ee56b3 MotorDriver_SU.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorDriver_SU.lib Wed Nov 19 13:08:25 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/NT32/code/MotorDriver_SU/#e6c391eb8fac
diff -r 000000000000 -r cc0c03ee56b3 center.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/center.cpp Wed Nov 19 13:08:25 2014 +0000
@@ -0,0 +1,138 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+
+DigitalIn sw(P0_1, PullUp);
+// trace L to R
+DigitalIn sensor1(P0_16);
+DigitalIn sensor2(P0_13);
+DigitalIn sensor3(P0_11);
+// lift L to R
+DigitalIn sensor4(P0_21);
+DigitalIn sensor5(P0_22);
+
+DigitalOut led(P1_28);
+
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+
+int main(){
+ while(1){
+
+ //int height = 0;//lift position
+ int check1 = 0;//convert count
+ int check2 = 0;
+ int count = 0;//count liftsensor B&B
+ while(sw == 0){
+ motor.Drive(0, STOP, 0); //R
+ motor.Drive(1, STOP, 0);//L
+ lift.Drive(0, STOP, 0);
+ }
+//////////////////////////////////////////////////////////
+ //need to change some numerical value after test
+//////////////////////////////////////////////////////////
+ while(sw == 1){
+ if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 1500);
+ }
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ lift.Drive(0, STOP, 0);
+ }
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){
+ if( count == 0){
+ lift.Drive(0, CW, 4095); //lift up
+ motor.Drive(0, STOP, 0); //stop
+ motor.Drive(1, STOP, 0);
+ wait(1);
+ motor.Drive(0, CW, 1000); //restart
+ motor.Drive(1, CW, 1000);
+ lift.Drive(0, CW, 4095); //lift up more
+ wait(0.7);
+ lift.Drive(0, STOP, 0);
+ //height = 1;//lift position high
+ count++;
+ }
+ //need to change count[1,2,3...] after test
+ else if(count == 1){
+ count++;
+ }
+ else if(count == 2){
+ count++;
+ }
+ else if(count == 3){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ lift.Drive(0, CCW, 2000);//lift down
+ wait(1.3);
+ motor.Drive(0, CCW, 4095);//back for 2 seconds
+ motor.Drive(1, CCW, 4095);
+ lift.Drive(0, STOP, 0);
+ wait(2);
+ //height = 0;//lift position low
+ count = 0;//reset
+ }
+ else{
+ ;
+ }
+ }
+ else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, STOP, 500);
+ }
+ else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 500);
+ }
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 1500);
+ }
+ else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+ motor.Drive(0, CCW, 500);
+ motor.Drive(1, CCW, 500);
+ }
+ /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
+ motor.Drive(0, CW, 2048);
+ motor.Drive(1, STOP, 0);
+ lift.Drive(0, STOP, 0);
+ }*/
+ //all body on line's outside
+ else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
+ motor.Drive(0, CCW, 200);
+ motor.Drive(1, CW, 2000);
+ lift.Drive(0, STOP, 0);
+ }
+ //select course condition
+ else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){
+ if(check1 == 0){
+ check1 = 1;
+ //enter the right courses
+ motor.Drive(0, CW, 500);//change both speed
+ motor.Drive(1, CW, 1500);
+ wait(1);
+ }
+ else{
+ ;//response only once
+ }
+ }
+ else if(sensor2 == 0 && sensor4 == 0 && sensor5 == 1){
+ if(check2 == 0){
+ check2 = 1;
+ //enter the center course
+ motor.Drive(0, CW, 1500);//change both speed
+ motor.Drive(1, CW, 500);
+ wait(1);
+ }
+ else{
+ ;//response only once
+ }
+ }
+ else{
+ motor.Drive(0, CW, 2500);//spin to search correct way
+ motor.Drive(1, CW, 500);
+ }
+ }
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r cc0c03ee56b3 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 19 13:08:25 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file