Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/Motors.h@8:ce5b1bf38077, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 23:58:04 2014 +0000
Revision:
8:ce5b1bf38077
Parent:
7:95ebadc83fc7 
asdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goy5022 0:c2ec30f28676 1
goy5022 2:997f57aee3b7 2 #ifndef MOTOR_H
goy5022 2:997f57aee3b7 3 #define MOTOR_H
goy5022 0:c2ec30f28676 4
goy5022 0:c2ec30f28676 5 #include "mbed.h"
goy5022 7:95ebadc83fc7 6 #include "Mapping.h"
goy5022 0:c2ec30f28676 7
goy5022 2:997f57aee3b7 8 #define turn_time 152
goy5022 4:ecfe2115e9a4 9 #define MOTOR_WAIT 600
goy5022 0:c2ec30f28676 10 #define forward_time 190
goy5022 0:c2ec30f28676 11
goy5022 7:95ebadc83fc7 12
goy5022 2:997f57aee3b7 13 //#include "Sensors.h"
goy5022 1:cfe6a6ad8dca 14
goy5022 1:cfe6a6ad8dca 15 Serial Motor(p13,p14);
goy5022 0:c2ec30f28676 16
goy5022 2:997f57aee3b7 17 int rSpeed = 0;
goy5022 2:997f57aee3b7 18 int lSpeed = 0;
goy5022 0:c2ec30f28676 19
goy5022 0:c2ec30f28676 20 void setRightSpeed(int speed)
goy5022 0:c2ec30f28676 21 {
goy5022 0:c2ec30f28676 22 if(speed >= 0)
goy5022 0:c2ec30f28676 23 Motor.printf("2f%i\r", speed);
goy5022 0:c2ec30f28676 24 else
goy5022 0:c2ec30f28676 25 Motor.printf("2r%i\r", (-speed));
goy5022 2:997f57aee3b7 26
goy5022 2:997f57aee3b7 27
goy5022 2:997f57aee3b7 28 rSpeed = speed;
goy5022 2:997f57aee3b7 29 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 30 }
goy5022 0:c2ec30f28676 31
goy5022 0:c2ec30f28676 32
goy5022 0:c2ec30f28676 33 void setLeftSpeed(int speed)
goy5022 0:c2ec30f28676 34 {
goy5022 0:c2ec30f28676 35 if(speed >= 0)
goy5022 0:c2ec30f28676 36 Motor.printf("1r%i\r", speed);
goy5022 0:c2ec30f28676 37 else
goy5022 0:c2ec30f28676 38 Motor.printf("1f%i\r", (-speed));
goy5022 2:997f57aee3b7 39
goy5022 2:997f57aee3b7 40 lSpeed = speed;
goy5022 2:997f57aee3b7 41 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 42 }
goy5022 2:997f57aee3b7 43
goy5022 2:997f57aee3b7 44 void stop_mov()
goy5022 0:c2ec30f28676 45 {
goy5022 3:1a8a7cc709cc 46 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 47 setLeftSpeed(0);
goy5022 2:997f57aee3b7 48 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 49 setRightSpeed(0);
goy5022 2:997f57aee3b7 50 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 51
goy5022 0:c2ec30f28676 52 }
goy5022 0:c2ec30f28676 53
goy5022 1:cfe6a6ad8dca 54 void forward(int f)
goy5022 1:cfe6a6ad8dca 55 {
goy5022 3:1a8a7cc709cc 56 wait_us(MOTOR_WAIT);
goy5022 1:cfe6a6ad8dca 57 setLeftSpeed(f-1);
goy5022 2:997f57aee3b7 58 wait_us(MOTOR_WAIT);
goy5022 1:cfe6a6ad8dca 59 setRightSpeed(f);
goy5022 2:997f57aee3b7 60 wait_us(MOTOR_WAIT);
goy5022 1:cfe6a6ad8dca 61 setLeftSpeed(f);
goy5022 2:997f57aee3b7 62 wait_us(MOTOR_WAIT);
goy5022 1:cfe6a6ad8dca 63
goy5022 7:95ebadc83fc7 64
goy5022 1:cfe6a6ad8dca 65
goy5022 2:997f57aee3b7 66 stop_mov();
goy5022 1:cfe6a6ad8dca 67 }
goy5022 1:cfe6a6ad8dca 68
goy5022 0:c2ec30f28676 69 void turn_right(int r)
goy5022 0:c2ec30f28676 70 {
goy5022 7:95ebadc83fc7 71
goy5022 7:95ebadc83fc7 72 wait(0.5);
goy5022 7:95ebadc83fc7 73 forward(3);
goy5022 7:95ebadc83fc7 74 wait_ms(320);
goy5022 2:997f57aee3b7 75 wait_us(MOTOR_WAIT);
goy5022 7:95ebadc83fc7 76 setLeftSpeed(-2);
goy5022 2:997f57aee3b7 77 wait_us(MOTOR_WAIT);
goy5022 7:95ebadc83fc7 78 setRightSpeed(-1);
goy5022 7:95ebadc83fc7 79 wait_ms(25);
goy5022 2:997f57aee3b7 80 stop_mov();
goy5022 2:997f57aee3b7 81
goy5022 7:95ebadc83fc7 82
goy5022 0:c2ec30f28676 83 }
goy5022 0:c2ec30f28676 84 void turn_left(int l)
goy5022 0:c2ec30f28676 85 {
goy5022 7:95ebadc83fc7 86 forward(2);
goy5022 7:95ebadc83fc7 87 wait_ms(200);
goy5022 7:95ebadc83fc7 88 stop_mov();
goy5022 0:c2ec30f28676 89 setLeftSpeed(-l);
goy5022 2:997f57aee3b7 90 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 91 setRightSpeed(l);
goy5022 7:95ebadc83fc7 92 wait_ms(turn_time); ///Set for turn speed
goy5022 0:c2ec30f28676 93 setRightSpeed(0);
goy5022 2:997f57aee3b7 94 wait_us(MOTOR_WAIT);
goy5022 0:c2ec30f28676 95 setLeftSpeed(0);
goy5022 2:997f57aee3b7 96 wait_us(MOTOR_WAIT);
goy5022 7:95ebadc83fc7 97 orientation_turnLeft();
goy5022 7:95ebadc83fc7 98 forward(2);
goy5022 7:95ebadc83fc7 99 wait_ms(200);
goy5022 7:95ebadc83fc7 100 stop_mov();
goy5022 0:c2ec30f28676 101 }
goy5022 7:95ebadc83fc7 102
goy5022 7:95ebadc83fc7 103 void turn_around(int a)
goy5022 7:95ebadc83fc7 104 {
goy5022 7:95ebadc83fc7 105 setLeftSpeed(-a);
goy5022 7:95ebadc83fc7 106 wait_us(MOTOR_WAIT);
goy5022 7:95ebadc83fc7 107 setRightSpeed(a);
goy5022 7:95ebadc83fc7 108 wait_ms(turn_time);
goy5022 7:95ebadc83fc7 109 wait_ms(turn_time); ///Set for turn speed
goy5022 7:95ebadc83fc7 110 setRightSpeed(0);
goy5022 7:95ebadc83fc7 111 wait_us(MOTOR_WAIT);
goy5022 7:95ebadc83fc7 112 setLeftSpeed(0);
goy5022 7:95ebadc83fc7 113 wait_us(MOTOR_WAIT);
goy5022 7:95ebadc83fc7 114 orientation_turnAround();
goy5022 7:95ebadc83fc7 115 }
goy5022 7:95ebadc83fc7 116
goy5022 3:1a8a7cc709cc 117 void handbrake()
goy5022 3:1a8a7cc709cc 118 {
goy5022 5:9e504a5a1f48 119 if(lSpeed > 0 && rSpeed > 0)
goy5022 5:9e504a5a1f48 120 {
goy5022 3:1a8a7cc709cc 121 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 122 setLeftSpeed(-3);
goy5022 3:1a8a7cc709cc 123 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 124 setRightSpeed(-4);
goy5022 3:1a8a7cc709cc 125 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 126 setLeftSpeed(-4);
goy5022 5:9e504a5a1f48 127 wait_ms(50);
goy5022 3:1a8a7cc709cc 128 setRightSpeed(-3);
goy5022 3:1a8a7cc709cc 129 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 130 setLeftSpeed(-4);
goy5022 3:1a8a7cc709cc 131 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 132 setLeftSpeed(-3);
goy5022 5:9e504a5a1f48 133 wait_ms(100);
goy5022 3:1a8a7cc709cc 134 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 135 setRightSpeed(-1);
goy5022 3:1a8a7cc709cc 136 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 137 setLeftSpeed(-1);
goy5022 3:1a8a7cc709cc 138 wait_ms(10);
goy5022 5:9e504a5a1f48 139 }
goy5022 3:1a8a7cc709cc 140 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 141 setRightSpeed(0);
goy5022 3:1a8a7cc709cc 142 wait_us(MOTOR_WAIT);
goy5022 3:1a8a7cc709cc 143 setLeftSpeed(0);
goy5022 4:ecfe2115e9a4 144 wait_us(MOTOR_WAIT);
goy5022 4:ecfe2115e9a4 145 setRightSpeed(0);
goy5022 4:ecfe2115e9a4 146 wait_us(MOTOR_WAIT);
goy5022 4:ecfe2115e9a4 147 setLeftSpeed(0);
goy5022 3:1a8a7cc709cc 148 }
goy5022 2:997f57aee3b7 149 #endif