Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/Motors.h@1:cfe6a6ad8dca, 2014-03-29 (annotated)

Committer:
goy5022
Date:
Sat Mar 29 13:25:23 2014 +0000
Revision:
1:cfe6a6ad8dca
Parent:
0:c2ec30f28676
Child:
2:997f57aee3b7
derp_1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goy5022 0:c2ec30f28676 1
goy5022 0:c2ec30f28676 2
goy5022 0:c2ec30f28676 3 #include "mbed.h"
goy5022 0:c2ec30f28676 4
goy5022 0:c2ec30f28676 5 #define turn_time 157
goy5022 1:cfe6a6ad8dca 6 #define motor_wait 700
goy5022 0:c2ec30f28676 7 #define forward_time 190
goy5022 0:c2ec30f28676 8
goy5022 1:cfe6a6ad8dca 9 #include "PID.h"
goy5022 1:cfe6a6ad8dca 10 #include "Sensors.h"
goy5022 1:cfe6a6ad8dca 11
goy5022 1:cfe6a6ad8dca 12 Serial Motor(p13,p14);
goy5022 0:c2ec30f28676 13
goy5022 0:c2ec30f28676 14
goy5022 0:c2ec30f28676 15
goy5022 0:c2ec30f28676 16
goy5022 0:c2ec30f28676 17 void setRightSpeed(int speed)
goy5022 0:c2ec30f28676 18 {
goy5022 0:c2ec30f28676 19 if(speed >= 0)
goy5022 0:c2ec30f28676 20 Motor.printf("2f%i\r", speed);
goy5022 0:c2ec30f28676 21 else
goy5022 0:c2ec30f28676 22 Motor.printf("2r%i\r", (-speed));
goy5022 0:c2ec30f28676 23 }
goy5022 0:c2ec30f28676 24
goy5022 0:c2ec30f28676 25
goy5022 0:c2ec30f28676 26 void setLeftSpeed(int speed)
goy5022 0:c2ec30f28676 27 {
goy5022 0:c2ec30f28676 28 if(speed >= 0)
goy5022 0:c2ec30f28676 29 Motor.printf("1r%i\r", speed);
goy5022 0:c2ec30f28676 30 else
goy5022 0:c2ec30f28676 31 Motor.printf("1f%i\r", (-speed));
goy5022 0:c2ec30f28676 32 }
goy5022 0:c2ec30f28676 33 void stop()
goy5022 0:c2ec30f28676 34 {
goy5022 1:cfe6a6ad8dca 35 setLeftSpeed(-5);
goy5022 1:cfe6a6ad8dca 36 wait_us(motor_wait);
goy5022 1:cfe6a6ad8dca 37 setRightSpeed(-5);
goy5022 1:cfe6a6ad8dca 38 wait_us(motor_wait);
goy5022 1:cfe6a6ad8dca 39 wait_us(20000);
goy5022 0:c2ec30f28676 40 setLeftSpeed(0);
goy5022 0:c2ec30f28676 41 wait_us(motor_wait);
goy5022 0:c2ec30f28676 42 setRightSpeed(0);
goy5022 0:c2ec30f28676 43 wait_us(motor_wait);
goy5022 0:c2ec30f28676 44
goy5022 0:c2ec30f28676 45 }
goy5022 0:c2ec30f28676 46
goy5022 1:cfe6a6ad8dca 47 void forward(int f)
goy5022 1:cfe6a6ad8dca 48 {
goy5022 1:cfe6a6ad8dca 49 setLeftSpeed(f-1);
goy5022 1:cfe6a6ad8dca 50 wait_us(motor_wait);
goy5022 1:cfe6a6ad8dca 51 setRightSpeed(f);
goy5022 1:cfe6a6ad8dca 52 wait_us(motor_wait);
goy5022 1:cfe6a6ad8dca 53 setLeftSpeed(f);
goy5022 1:cfe6a6ad8dca 54 wait_us(motor_wait);
goy5022 1:cfe6a6ad8dca 55
goy5022 1:cfe6a6ad8dca 56 while(1)
goy5022 1:cfe6a6ad8dca 57 {
goy5022 1:cfe6a6ad8dca 58 WIRELESS.printf("L:%f R:%f ::: PID: %f \n\r", valL, valR, PID());
goy5022 1:cfe6a6ad8dca 59
goy5022 1:cfe6a6ad8dca 60 }
goy5022 1:cfe6a6ad8dca 61
goy5022 1:cfe6a6ad8dca 62 stop();
goy5022 1:cfe6a6ad8dca 63 }
goy5022 1:cfe6a6ad8dca 64
goy5022 1:cfe6a6ad8dca 65
goy5022 0:c2ec30f28676 66
goy5022 0:c2ec30f28676 67
goy5022 0:c2ec30f28676 68 void turn_right(int r)
goy5022 0:c2ec30f28676 69 {
goy5022 0:c2ec30f28676 70 setLeftSpeed(r);
goy5022 0:c2ec30f28676 71 wait_us(motor_wait);
goy5022 0:c2ec30f28676 72 setRightSpeed(-r);
goy5022 0:c2ec30f28676 73 wait_ms(turn_time); ///Set for turn speed
goy5022 0:c2ec30f28676 74 stop();
goy5022 0:c2ec30f28676 75 }
goy5022 0:c2ec30f28676 76 void turn_left(int l)
goy5022 0:c2ec30f28676 77 {
goy5022 0:c2ec30f28676 78 setLeftSpeed(-l);
goy5022 0:c2ec30f28676 79 wait_us(motor_wait);
goy5022 0:c2ec30f28676 80 setRightSpeed(l);
goy5022 0:c2ec30f28676 81 wait_ms(turn_time); ///Set for turn speed
goy5022 0:c2ec30f28676 82 setRightSpeed(0);
goy5022 0:c2ec30f28676 83 wait_us(motor_wait);
goy5022 0:c2ec30f28676 84 setLeftSpeed(0);
goy5022 0:c2ec30f28676 85 wait_us(motor_wait);
goy5022 0:c2ec30f28676 86 }