asdf
Dependencies: L3GD20 LSM303DLHC mbed
Headers/Motors.h
- Committer:
- goy5022
- Date:
- 2014-04-03
- Revision:
- 8:ce5b1bf38077
- Parent:
- 7:95ebadc83fc7
File content as of revision 8:ce5b1bf38077:
#ifndef MOTOR_H #define MOTOR_H #include "mbed.h" #include "Mapping.h" #define turn_time 152 #define MOTOR_WAIT 600 #define forward_time 190 //#include "Sensors.h" Serial Motor(p13,p14); int rSpeed = 0; int lSpeed = 0; void setRightSpeed(int speed) { if(speed >= 0) Motor.printf("2f%i\r", speed); else Motor.printf("2r%i\r", (-speed)); rSpeed = speed; wait_us(MOTOR_WAIT); } void setLeftSpeed(int speed) { if(speed >= 0) Motor.printf("1r%i\r", speed); else Motor.printf("1f%i\r", (-speed)); lSpeed = speed; wait_us(MOTOR_WAIT); } void stop_mov() { wait_us(MOTOR_WAIT); setLeftSpeed(0); wait_us(MOTOR_WAIT); setRightSpeed(0); wait_us(MOTOR_WAIT); } void forward(int f) { wait_us(MOTOR_WAIT); setLeftSpeed(f-1); wait_us(MOTOR_WAIT); setRightSpeed(f); wait_us(MOTOR_WAIT); setLeftSpeed(f); wait_us(MOTOR_WAIT); stop_mov(); } void turn_right(int r) { wait(0.5); forward(3); wait_ms(320); wait_us(MOTOR_WAIT); setLeftSpeed(-2); wait_us(MOTOR_WAIT); setRightSpeed(-1); wait_ms(25); stop_mov(); } void turn_left(int l) { forward(2); wait_ms(200); stop_mov(); setLeftSpeed(-l); wait_us(MOTOR_WAIT); setRightSpeed(l); wait_ms(turn_time); ///Set for turn speed setRightSpeed(0); wait_us(MOTOR_WAIT); setLeftSpeed(0); wait_us(MOTOR_WAIT); orientation_turnLeft(); forward(2); wait_ms(200); stop_mov(); } void turn_around(int a) { setLeftSpeed(-a); wait_us(MOTOR_WAIT); setRightSpeed(a); wait_ms(turn_time); wait_ms(turn_time); ///Set for turn speed setRightSpeed(0); wait_us(MOTOR_WAIT); setLeftSpeed(0); wait_us(MOTOR_WAIT); orientation_turnAround(); } void handbrake() { if(lSpeed > 0 && rSpeed > 0) { wait_us(MOTOR_WAIT); setLeftSpeed(-3); wait_us(MOTOR_WAIT); setRightSpeed(-4); wait_us(MOTOR_WAIT); setLeftSpeed(-4); wait_ms(50); setRightSpeed(-3); wait_us(MOTOR_WAIT); setLeftSpeed(-4); wait_us(MOTOR_WAIT); setLeftSpeed(-3); wait_ms(100); wait_us(MOTOR_WAIT); setRightSpeed(-1); wait_us(MOTOR_WAIT); setLeftSpeed(-1); wait_ms(10); } wait_us(MOTOR_WAIT); setRightSpeed(0); wait_us(MOTOR_WAIT); setLeftSpeed(0); wait_us(MOTOR_WAIT); setRightSpeed(0); wait_us(MOTOR_WAIT); setLeftSpeed(0); } #endif