Motors.h

00001 
00002 #ifndef MOTOR_H
00003 #define MOTOR_H
00004 
00005 #include "mbed.h"
00006 #include "Mapping.h"
00007 
00008 #define turn_time 152
00009 #define MOTOR_WAIT 600
00010 #define forward_time 190
00011 
00012 
00013 //#include "Sensors.h"
00014 
00015 Serial Motor(p13,p14);
00016 
00017 int rSpeed = 0;
00018 int lSpeed = 0;
00019 
00020 void setRightSpeed(int speed)
00021 {
00022     if(speed >= 0)
00023         Motor.printf("2f%i\r", speed);
00024     else
00025         Motor.printf("2r%i\r", (-speed));
00026         
00027         
00028     rSpeed = speed;
00029     wait_us(MOTOR_WAIT);
00030 }
00031 
00032 
00033 void setLeftSpeed(int speed)
00034 {
00035     if(speed >= 0)
00036         Motor.printf("1r%i\r", speed);
00037     else
00038         Motor.printf("1f%i\r", (-speed));
00039         
00040     lSpeed = speed;
00041     wait_us(MOTOR_WAIT);
00042 }
00043 
00044 void stop_mov()
00045 {
00046     wait_us(MOTOR_WAIT);
00047     setLeftSpeed(0);
00048     wait_us(MOTOR_WAIT);
00049     setRightSpeed(0);
00050     wait_us(MOTOR_WAIT);
00051   
00052 }
00053 
00054 void forward(int f)
00055 {
00056     wait_us(MOTOR_WAIT);
00057     setLeftSpeed(f-1);
00058     wait_us(MOTOR_WAIT);
00059     setRightSpeed(f);
00060     wait_us(MOTOR_WAIT);
00061     setLeftSpeed(f);
00062     wait_us(MOTOR_WAIT);
00063     
00064     
00065     
00066     stop_mov();
00067 }
00068 
00069 void turn_right(int r)
00070 {
00071     
00072     wait(0.5);
00073     forward(3);
00074     wait_ms(320);
00075     wait_us(MOTOR_WAIT);
00076     setLeftSpeed(-2);
00077     wait_us(MOTOR_WAIT);
00078     setRightSpeed(-1);
00079     wait_ms(25);
00080     stop_mov();
00081     
00082 
00083 }
00084 void turn_left(int l)
00085 {
00086     forward(2);
00087     wait_ms(200);
00088     stop_mov();
00089     setLeftSpeed(-l);
00090     wait_us(MOTOR_WAIT);
00091     setRightSpeed(l);
00092     wait_ms(turn_time);       ///Set for turn speed
00093     setRightSpeed(0);
00094     wait_us(MOTOR_WAIT);
00095     setLeftSpeed(0);
00096     wait_us(MOTOR_WAIT);
00097     orientation_turnLeft();
00098     forward(2);
00099     wait_ms(200);
00100     stop_mov();
00101 }
00102 
00103 void turn_around(int a)
00104 {
00105     setLeftSpeed(-a);
00106     wait_us(MOTOR_WAIT);
00107     setRightSpeed(a);
00108     wait_ms(turn_time);  
00109     wait_ms(turn_time);     ///Set for turn speed
00110     setRightSpeed(0);
00111     wait_us(MOTOR_WAIT);
00112     setLeftSpeed(0);
00113     wait_us(MOTOR_WAIT);
00114     orientation_turnAround();
00115 }
00116 
00117 void handbrake()
00118 {
00119     if(lSpeed > 0 && rSpeed > 0)
00120     {
00121     wait_us(MOTOR_WAIT);
00122     setLeftSpeed(-3);
00123     wait_us(MOTOR_WAIT);
00124     setRightSpeed(-4);
00125     wait_us(MOTOR_WAIT);
00126     setLeftSpeed(-4);
00127     wait_ms(50);
00128     setRightSpeed(-3);
00129     wait_us(MOTOR_WAIT);
00130     setLeftSpeed(-4);
00131     wait_us(MOTOR_WAIT);
00132     setLeftSpeed(-3);
00133     wait_ms(100);
00134     wait_us(MOTOR_WAIT);
00135     setRightSpeed(-1);
00136     wait_us(MOTOR_WAIT);
00137     setLeftSpeed(-1);
00138     wait_ms(10);
00139     }
00140     wait_us(MOTOR_WAIT);
00141     setRightSpeed(0);
00142     wait_us(MOTOR_WAIT);
00143     setLeftSpeed(0);
00144     wait_us(MOTOR_WAIT);
00145     setRightSpeed(0);
00146     wait_us(MOTOR_WAIT);
00147     setLeftSpeed(0);
00148 }
00149 #endif