Mark Vandermeulen / Mbed 2 deprecated Rotation_test

For the SUTD research project. This is a sample code to access the MPU6050

Dependencies:   MPU6050 Terminal mbed

main.cpp@0:59f6f94baeb1, 2013-12-01 (annotated)

Committer:
majik
Date:
Sun Dec 01 06:22:13 2013 +0000
Revision:
0:59f6f94baeb1
Robot orientation test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:59f6f94baeb1 1 /** This program constantly sends the accelerometer data over the bluetooth to the computer.
majik 0:59f6f94baeb1 2 * The robot will drive FWD and REVERSE at maximum speed when you press W and S. //Turn 90 degrees at a time//
majik 0:59f6f94baeb1 3 * TODO: implement 90deg rotation
majik 0:59f6f94baeb1 4 * TODO: Get rid of flags. Turn everything into a class
majik 0:59f6f94baeb1 5 * TODO: Use a PID controller for turning instead of whatever mess you used today
majik 0:59f6f94baeb1 6 */
majik 0:59f6f94baeb1 7 #include "mbed.h"
majik 0:59f6f94baeb1 8 #include "MPU6050.h"
majik 0:59f6f94baeb1 9 #include "Terminal.h"
majik 0:59f6f94baeb1 10 #include "string"
majik 0:59f6f94baeb1 11 #include "main.h"
majik 0:59f6f94baeb1 12
majik 0:59f6f94baeb1 13 //const int startMotor = 2; //time (in seconds) when motor starts
majik 0:59f6f94baeb1 14 //const int stopMotor = 4; //time (in seconds) when motor stops
majik 0:59f6f94baeb1 15
majik 0:59f6f94baeb1 16 Terminal pc(PTA2,PTA1); //bluetooth
majik 0:59f6f94baeb1 17
majik 0:59f6f94baeb1 18 DigitalOut myled(PTE3); //LED
majik 0:59f6f94baeb1 19
majik 0:59f6f94baeb1 20 MPU6050 mpu(PTE0, PTE1); //Accelerometer (needs the MPU6050 library)
majik 0:59f6f94baeb1 21
majik 0:59f6f94baeb1 22 //things for the motor controller
majik 0:59f6f94baeb1 23 PwmOut PWMA(PTA4);
majik 0:59f6f94baeb1 24 PwmOut PWMB(PTA5);
majik 0:59f6f94baeb1 25 DigitalOut AIN1(PTA14);
majik 0:59f6f94baeb1 26 DigitalOut AIN2(PTA13);
majik 0:59f6f94baeb1 27 DigitalOut BIN1(PTA16);
majik 0:59f6f94baeb1 28 DigitalOut BIN2(PTA17);
majik 0:59f6f94baeb1 29 DigitalOut STBY(PTA15);
majik 0:59f6f94baeb1 30
majik 0:59f6f94baeb1 31 //motor functions
majik 0:59f6f94baeb1 32 void motor_control(bool motor, int speed, bool dir); //speed should be between 1 and 100
majik 0:59f6f94baeb1 33 void stop();
majik 0:59f6f94baeb1 34 void straight(int speed); //drive robot straight. Speed between -100 and 100. Accelerometer works best at 100
majik 0:59f6f94baeb1 35 void turn_left(int speedA, int speedB);
majik 0:59f6f94baeb1 36 void turn_right(int speedA, int speedB);
majik 0:59f6f94baeb1 37
majik 0:59f6f94baeb1 38 double integrate(data *prev, int acc[3], int time); //integrate. this is not implemented yet
majik 0:59f6f94baeb1 39 void remote_control(char c, int *turn); //Send a cmmand to this function, it will drive the robot
majik 0:59f6f94baeb1 40
majik 0:59f6f94baeb1 41 int accFlag = 0; //flag to continue acceleration or deceleration (1 for acc, 0 for none, -1 for dec)
majik 0:59f6f94baeb1 42 int turnFlag = 0; //flag for direction of turn. If 0, no turn, if 1 turn right, -1 turn left
majik 0:59f6f94baeb1 43 int accLimit = 16384.0/9.81* 1; //acceleration limit = 2 m/s2
majik 0:59f6f94baeb1 44 int maxRot = 0;
majik 0:59f6f94baeb1 45
majik 0:59f6f94baeb1 46
majik 0:59f6f94baeb1 47 int main()
majik 0:59f6f94baeb1 48 {
majik 0:59f6f94baeb1 49 double t2 = 0;
majik 0:59f6f94baeb1 50 double tnow;
majik 0:59f6f94baeb1 51 double SP;
majik 0:59f6f94baeb1 52 int Kp = 75; //proportional gain (tuning)
majik 0:59f6f94baeb1 53 int Ki = 0; //integral gain (tuning)
majik 0:59f6f94baeb1 54 double Kd = 0; //derivative gain (tuning) *Not used = 0*
majik 0:59f6f94baeb1 55 double Irz = 0;
majik 0:59f6f94baeb1 56
majik 0:59f6f94baeb1 57 double rotationSpeed=0;
majik 0:59f6f94baeb1 58 double rz=0; //rotation in the z axis
majik 0:59f6f94baeb1 59 double tprev=0;//time of previous iteration
majik 0:59f6f94baeb1 60 double pi = 3.14159265359;
majik 0:59f6f94baeb1 61 int i=0;
majik 0:59f6f94baeb1 62 int turnTarget = 0; //angle for the robot to drive at (in degrees)
majik 0:59f6f94baeb1 63 // data *state; //this will be passed into the integrate fucntion for recursiveness
majik 0:59f6f94baeb1 64 //send data to the integrate function, delayed by 10 samples
majik 0:59f6f94baeb1 65 //
majik 0:59f6f94baeb1 66 char c;
majik 0:59f6f94baeb1 67 // int flag = 0;
majik 0:59f6f94baeb1 68 int accdata[3];
majik 0:59f6f94baeb1 69 int avAcc[3];
majik 0:59f6f94baeb1 70 int gyrodata[3];
majik 0:59f6f94baeb1 71 Timer t;
majik 0:59f6f94baeb1 72 pc.baud(115200); //Make sure this baud rate matches the baud rate of the bluetooth (set baud rate with "BluetoothAT")
majik 0:59f6f94baeb1 73 wait(0.1);
majik 0:59f6f94baeb1 74 char test = mpu.testConnection();
majik 0:59f6f94baeb1 75
majik 0:59f6f94baeb1 76
majik 0:59f6f94baeb1 77 pc.baud(115200); //this can only be changed after the BLUETOOTH baud rate has been changed
majik 0:59f6f94baeb1 78 if (mpu.testConnection()){
majik 0:59f6f94baeb1 79 //pc.printf("MPU connection succeeded\n\r");
majik 0:59f6f94baeb1 80 myled = 0;
majik 0:59f6f94baeb1 81 }
majik 0:59f6f94baeb1 82 else{
majik 0:59f6f94baeb1 83 pc.printf("MPU connection failed\n\r"); //Todo: If connection fails, retry a couple times. Try resetting MPU (this would need another wire?)
majik 0:59f6f94baeb1 84 myled = 1;
majik 0:59f6f94baeb1 85 }
majik 0:59f6f94baeb1 86 wait(3);
majik 0:59f6f94baeb1 87
majik 0:59f6f94baeb1 88 t.reset();
majik 0:59f6f94baeb1 89 // mpu.setBW(MPU6050_BW_10); //set bandwidth of low pass filter for Accerlerometer AND Gyro.
majik 0:59f6f94baeb1 90 // mpu.setBW(MPU6050_BW_42);
majik 0:59f6f94baeb1 91 // mpu.setBW(MPU6050_BW_98);
majik 0:59f6f94baeb1 92 mpu.setBW(MPU6050_BW_20);
majik 0:59f6f94baeb1 93 // mpu.setGyroRange(MPU6050_GYRO_RANGE_250); //To convert gyro output to RAD/S, divide by 7505.7
majik 0:59f6f94baeb1 94 mpu.setGyroRange(MPU6050_GYRO_RANGE_500); //to convert gyro output to RAD/S, divide by 3752.9
majik 0:59f6f94baeb1 95 //double gyroCorrect = 3752.9; //////change this number!!!!Decrease to decrease rotation
majik 0:59f6f94baeb1 96 double gyroCorrect = 3720; //prev3754.82prev3752.9 Use this number to convert gyro reading to rad/s
majik 0:59f6f94baeb1 97 int count = 0;
majik 0:59f6f94baeb1 98 double rzOff = 0;
majik 0:59f6f94baeb1 99 while(t.read()<1) //calculate gyro offset
majik 0:59f6f94baeb1 100 {
majik 0:59f6f94baeb1 101 mpu.getGyroRaw(gyrodata);
majik 0:59f6f94baeb1 102 rzOff += gyrodata[2];
majik 0:59f6f94baeb1 103 count++;
majik 0:59f6f94baeb1 104 }
majik 0:59f6f94baeb1 105 rzOff = rzOff/count; //rzOffset
majik 0:59f6f94baeb1 106 while(1)
majik 0:59f6f94baeb1 107 {
majik 0:59f6f94baeb1 108 //speed = 100;
majik 0:59f6f94baeb1 109 mpu.getAcceleroRaw(accdata);
majik 0:59f6f94baeb1 110 mpu.getGyroRaw(gyrodata);
majik 0:59f6f94baeb1 111
majik 0:59f6f94baeb1 112 //ROTATION//------------------------------------//
majik 0:59f6f94baeb1 113
majik 0:59f6f94baeb1 114 //MV = 0; //input (speed setting)
majik 0:59f6f94baeb1 115 SP = turnTarget*pi/180; //desired angle (radians)
majik 0:59f6f94baeb1 116 tnow = t.read();
majik 0:59f6f94baeb1 117 rz = rz + ((gyrodata[2]-rzOff)*(tnow-tprev)/gyroCorrect); //rz is the rotation(radians) from start
majik 0:59f6f94baeb1 118 Irz = Irz + (rz-SP)*(tnow-tprev); //Irz needs to be reset every so often, or it should be ignored
majik 0:59f6f94baeb1 119 tprev = tnow;
majik 0:59f6f94baeb1 120 rotationSpeed = (int(Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect));
majik 0:59f6f94baeb1 121 pc.printf("\n\r%f\t",rotationSpeed);
majik 0:59f6f94baeb1 122 if(maxRot < gyrodata[2])
majik 0:59f6f94baeb1 123 maxRot = gyrodata[2];
majik 0:59f6f94baeb1 124
majik 0:59f6f94baeb1 125
majik 0:59f6f94baeb1 126
majik 0:59f6f94baeb1 127
majik 0:59f6f94baeb1 128
majik 0:59f6f94baeb1 129
majik 0:59f6f94baeb1 130 if(abs(rotationSpeed)<1)//(abs(rz-SP)<(0.0175*gyroCorrect))
majik 0:59f6f94baeb1 131 {
majik 0:59f6f94baeb1 132 rotationSpeed = 0;
majik 0:59f6f94baeb1 133 Irz = 0;
majik 0:59f6f94baeb1 134 }
majik 0:59f6f94baeb1 135 else if(abs(rotationSpeed)<10)
majik 0:59f6f94baeb1 136 {
majik 0:59f6f94baeb1 137
majik 0:59f6f94baeb1 138 if (rotationSpeed>0)
majik 0:59f6f94baeb1 139 rotationSpeed = 10;
majik 0:59f6f94baeb1 140 else
majik 0:59f6f94baeb1 141 rotationSpeed = -10;
majik 0:59f6f94baeb1 142 }
majik 0:59f6f94baeb1 143 if(tnow>t2)
majik 0:59f6f94baeb1 144 {
majik 0:59f6f94baeb1 145 turn_right(-rotationSpeed,rotationSpeed);
majik 0:59f6f94baeb1 146 t2 = tnow+0.01;
majik 0:59f6f94baeb1 147 }
majik 0:59f6f94baeb1 148
majik 0:59f6f94baeb1 149
majik 0:59f6f94baeb1 150 //END ROTATION//---------------------------------//
majik 0:59f6f94baeb1 151 if (accFlag > 0)
majik 0:59f6f94baeb1 152 accFlag --;
majik 0:59f6f94baeb1 153 else if(accFlag < 0)
majik 0:59f6f94baeb1 154 accFlag++;
majik 0:59f6f94baeb1 155
majik 0:59f6f94baeb1 156 // avAcc[0] = 0;
majik 0:59f6f94baeb1 157 // avAcc[1] = 0;
majik 0:59f6f94baeb1 158 // avAcc[2] = 0;
majik 0:59f6f94baeb1 159 // for(i = 0; i<4; i++)
majik 0:59f6f94baeb1 160 // {
majik 0:59f6f94baeb1 161 //
majik 0:59f6f94baeb1 162 // mpu.getAcceleroRaw(accdata);
majik 0:59f6f94baeb1 163 // mpu.getGyroRaw(gyrodata);
majik 0:59f6f94baeb1 164 //
majik 0:59f6f94baeb1 165 // avAcc[0] += accdata[0];
majik 0:59f6f94baeb1 166 // avAcc[1] += accdata[1];
majik 0:59f6f94baeb1 167 // avAcc[2] += accdata[2];
majik 0:59f6f94baeb1 168 // }
majik 0:59f6f94baeb1 169
majik 0:59f6f94baeb1 170
majik 0:59f6f94baeb1 171 pc.printf("%f\t%d\t%d\t%d\t", tnow,accdata[0], accdata[1], accdata[2]);
majik 0:59f6f94baeb1 172 // pc.printf("%d\t%d\t%d\t%d\n\r", gyrodata[0], gyrodata[1], gyrodata[2],STBY.read());
majik 0:59f6f94baeb1 173 // pc.printf("%f\n\r",tnow);
majik 0:59f6f94baeb1 174
majik 0:59f6f94baeb1 175 //Old Code for moving the robot//
majik 0:59f6f94baeb1 176 /*
majik 0:59f6f94baeb1 177 if (t.read() > startMotor && flag ==0){
majik 0:59f6f94baeb1 178 myled = 0;
majik 0:59f6f94baeb1 179 straight(100);
majik 0:59f6f94baeb1 180 flag = 1;
majik 0:59f6f94baeb1 181 }
majik 0:59f6f94baeb1 182 if (t.read() > stopMotor){
majik 0:59f6f94baeb1 183 stop();
majik 0:59f6f94baeb1 184 myled = 1;
majik 0:59f6f94baeb1 185 STBY=0;
majik 0:59f6f94baeb1 186 flag = 2;
majik 0:59f6f94baeb1 187 }
majik 0:59f6f94baeb1 188 if (t.read() > 20)
majik 0:59f6f94baeb1 189 break;
majik 0:59f6f94baeb1 190 */
majik 0:59f6f94baeb1 191 //New code for moving robot//
majik 0:59f6f94baeb1 192 if(pc.readable() || accFlag){
majik 0:59f6f94baeb1 193 if(accFlag)
majik 0:59f6f94baeb1 194 c = 'i';
majik 0:59f6f94baeb1 195 else
majik 0:59f6f94baeb1 196 c = pc.getc();
majik 0:59f6f94baeb1 197 remote_control(c,&turnTarget);
majik 0:59f6f94baeb1 198 if (c=='t'){ //reset timer if t is pressed
majik 0:59f6f94baeb1 199 wait(2);
majik 0:59f6f94baeb1 200 t.reset();
majik 0:59f6f94baeb1 201 turnTarget = 0;
majik 0:59f6f94baeb1 202 }
majik 0:59f6f94baeb1 203 }
majik 0:59f6f94baeb1 204 }
majik 0:59f6f94baeb1 205
majik 0:59f6f94baeb1 206 }
majik 0:59f6f94baeb1 207 void motor_control(bool motor, int speed, bool dir) //motor: 0=right, 1=left; speed: 0-100; dir 0=fwd, 1=bkwd
majik 0:59f6f94baeb1 208 {
majik 0:59f6f94baeb1 209 if (speed == 0) //stop//
majik 0:59f6f94baeb1 210 {
majik 0:59f6f94baeb1 211 STBY = 0;
majik 0:59f6f94baeb1 212 if (motor ==0)
majik 0:59f6f94baeb1 213 {
majik 0:59f6f94baeb1 214 AIN1 = 0;
majik 0:59f6f94baeb1 215 AIN2 = 0;
majik 0:59f6f94baeb1 216 PWMA = 0;
majik 0:59f6f94baeb1 217 }
majik 0:59f6f94baeb1 218 else
majik 0:59f6f94baeb1 219 {
majik 0:59f6f94baeb1 220 BIN1 = 0;
majik 0:59f6f94baeb1 221 BIN2 = 0;
majik 0:59f6f94baeb1 222 PWMB = 0;
majik 0:59f6f94baeb1 223 }
majik 0:59f6f94baeb1 224 }
majik 0:59f6f94baeb1 225 else
majik 0:59f6f94baeb1 226 STBY = 1;
majik 0:59f6f94baeb1 227 if (!dir) //forward//
majik 0:59f6f94baeb1 228 {
majik 0:59f6f94baeb1 229 if(motor == 0) //right motor//
majik 0:59f6f94baeb1 230 {
majik 0:59f6f94baeb1 231 AIN1 = 1;
majik 0:59f6f94baeb1 232 AIN2 = 0;
majik 0:59f6f94baeb1 233 PWMA = abs(speed/100.0);
majik 0:59f6f94baeb1 234 }
majik 0:59f6f94baeb1 235 else //left motor//
majik 0:59f6f94baeb1 236 {
majik 0:59f6f94baeb1 237 BIN1 = 0; //REVERSE THESE WHEN FIX WIRING//////*****//////
majik 0:59f6f94baeb1 238 BIN2 = 1;
majik 0:59f6f94baeb1 239 PWMB = abs(speed/100.0);
majik 0:59f6f94baeb1 240 }
majik 0:59f6f94baeb1 241 }
majik 0:59f6f94baeb1 242 else
majik 0:59f6f94baeb1 243 {
majik 0:59f6f94baeb1 244 if(motor == 0) //right motor//
majik 0:59f6f94baeb1 245 {
majik 0:59f6f94baeb1 246 AIN1 = 0;
majik 0:59f6f94baeb1 247 AIN2 = 1;
majik 0:59f6f94baeb1 248 PWMA = abs(speed/100.0);
majik 0:59f6f94baeb1 249 }
majik 0:59f6f94baeb1 250 else //left motor//
majik 0:59f6f94baeb1 251 {
majik 0:59f6f94baeb1 252 BIN1 = 1; //REVERSE THESE WHEN FIXED WIRING!!////*****////////
majik 0:59f6f94baeb1 253 BIN2 = 0;
majik 0:59f6f94baeb1 254 PWMB = abs(speed/100.0);
majik 0:59f6f94baeb1 255 }
majik 0:59f6f94baeb1 256 }
majik 0:59f6f94baeb1 257
majik 0:59f6f94baeb1 258 }
majik 0:59f6f94baeb1 259 void stop()
majik 0:59f6f94baeb1 260 {
majik 0:59f6f94baeb1 261 motor_control(0,0,0); //motorA, speed=0, direction = fwd
majik 0:59f6f94baeb1 262 motor_control(1,0,0);
majik 0:59f6f94baeb1 263 STBY=0;
majik 0:59f6f94baeb1 264 }
majik 0:59f6f94baeb1 265 void straight(int speed) //if speed is +ve, go straight. -ve reverse
majik 0:59f6f94baeb1 266 {
majik 0:59f6f94baeb1 267 if (speed == 0)
majik 0:59f6f94baeb1 268 stop();
majik 0:59f6f94baeb1 269 else if (speed > 100)
majik 0:59f6f94baeb1 270 speed = 100;
majik 0:59f6f94baeb1 271 else if (speed < -100)
majik 0:59f6f94baeb1 272 speed = -100;
majik 0:59f6f94baeb1 273 else if (speed>0) //fwd//
majik 0:59f6f94baeb1 274 {
majik 0:59f6f94baeb1 275 motor_control(0,speed,0);
majik 0:59f6f94baeb1 276 motor_control(1,speed,0);
majik 0:59f6f94baeb1 277 }
majik 0:59f6f94baeb1 278 else
majik 0:59f6f94baeb1 279 {
majik 0:59f6f94baeb1 280 motor_control(0,speed,1);
majik 0:59f6f94baeb1 281 motor_control(1,speed,1);
majik 0:59f6f94baeb1 282 }
majik 0:59f6f94baeb1 283 }
majik 0:59f6f94baeb1 284 double integrate(int *prev, int acc[3], int time)
majik 0:59f6f94baeb1 285 {
majik 0:59f6f94baeb1 286 /*Returns the distance travelled*/
majik 0:59f6f94baeb1 287 return 0;
majik 0:59f6f94baeb1 288 }
majik 0:59f6f94baeb1 289
majik 0:59f6f94baeb1 290
majik 0:59f6f94baeb1 291 /*Makes robot go straight, stop, and turn*/
majik 0:59f6f94baeb1 292 void remote_control(char c, int *turn)
majik 0:59f6f94baeb1 293 {
majik 0:59f6f94baeb1 294 int speed;
majik 0:59f6f94baeb1 295 int y;
majik 0:59f6f94baeb1 296 switch (c)
majik 0:59f6f94baeb1 297 {
majik 0:59f6f94baeb1 298 case 'w': //forward - increment speed
majik 0:59f6f94baeb1 299 // turn=0;
majik 0:59f6f94baeb1 300 speed = 100;
majik 0:59f6f94baeb1 301 straight(speed);
majik 0:59f6f94baeb1 302 break;
majik 0:59f6f94baeb1 303 case 's': //backwards
majik 0:59f6f94baeb1 304 // turn=0;
majik 0:59f6f94baeb1 305 speed = -100;
majik 0:59f6f94baeb1 306 straight(speed);
majik 0:59f6f94baeb1 307 break;
majik 0:59f6f94baeb1 308 case 'd': //right
majik 0:59f6f94baeb1 309 /*if(BIN1.read()==0 && AIN1.read()==0) //if previously turning right, go straight
majik 0:59f6f94baeb1 310 stop();
majik 0:59f6f94baeb1 311 //{
majik 0:59f6f94baeb1 312 // turn = 0;
majik 0:59f6f94baeb1 313 // turnRate = 0;
majik 0:59f6f94baeb1 314 // straight(speed);
majik 0:59f6f94baeb1 315 //}
majik 0:59f6f94baeb1 316 else
majik 0:59f6f94baeb1 317 {
majik 0:59f6f94baeb1 318 //turn_left(50,-50);
majik 0:59f6f94baeb1 319 }*/
majik 0:59f6f94baeb1 320 *turn+=90; //rotate right 90 degrees
majik 0:59f6f94baeb1 321 break;
majik 0:59f6f94baeb1 322 case 'a': //left
majik 0:59f6f94baeb1 323 /*if(BIN1.read() && AIN1.read()) //if previously turning left, go straight;
majik 0:59f6f94baeb1 324 stop();
majik 0:59f6f94baeb1 325 else
majik 0:59f6f94baeb1 326 turn_right(-50,50);
majik 0:59f6f94baeb1 327 */
majik 0:59f6f94baeb1 328 *turn-=90; //rotate left 90 degrees
majik 0:59f6f94baeb1 329 break;
majik 0:59f6f94baeb1 330 case 'i': //constant acceleration
majik 0:59f6f94baeb1 331 //if(BIN1.read() || (AIN!.read() == 0)) //if previously reverse, stop
majik 0:59f6f94baeb1 332 // stop();
majik 0:59f6f94baeb1 333 //else //go in reverse, at a constant acceleration
majik 0:59f6f94baeb1 334 speed = (PWMA.read() + PWMB.read())/2*100;
majik 0:59f6f94baeb1 335 y = mpu.getAcceleroRawY();
majik 0:59f6f94baeb1 336 if(speed<100 || BIN1.read() || (AIN1.read()==0) && accFlag < 2)
majik 0:59f6f94baeb1 337 {
majik 0:59f6f94baeb1 338 accFlag = 5;
majik 0:59f6f94baeb1 339 if(BIN1.read()) stop();
majik 0:59f6f94baeb1 340 if(abs(y)<accLimit)
majik 0:59f6f94baeb1 341 {
majik 0:59f6f94baeb1 342 speed = speed + 1;
majik 0:59f6f94baeb1 343 straight(speed);
majik 0:59f6f94baeb1 344 }
majik 0:59f6f94baeb1 345 }
majik 0:59f6f94baeb1 346 else
majik 0:59f6f94baeb1 347 accFlag = 0;
majik 0:59f6f94baeb1 348 break;
majik 0:59f6f94baeb1 349 default: //stop
majik 0:59f6f94baeb1 350 speed = 0;
majik 0:59f6f94baeb1 351 stop();
majik 0:59f6f94baeb1 352 }
majik 0:59f6f94baeb1 353 }
majik 0:59f6f94baeb1 354
majik 0:59f6f94baeb1 355
majik 0:59f6f94baeb1 356 void turn_left(int speedA, int speedB)
majik 0:59f6f94baeb1 357 {
majik 0:59f6f94baeb1 358
majik 0:59f6f94baeb1 359 if (speedA < 0) //R reverse//
majik 0:59f6f94baeb1 360 {
majik 0:59f6f94baeb1 361 motor_control(0,speedA,1); //Right reverse
majik 0:59f6f94baeb1 362 motor_control(1,speedB,1); //Left reverse
majik 0:59f6f94baeb1 363 }
majik 0:59f6f94baeb1 364 else if(speedB <= 0)
majik 0:59f6f94baeb1 365 {
majik 0:59f6f94baeb1 366 motor_control(1,speedB,1); //Left reverse//
majik 0:59f6f94baeb1 367 motor_control(0,speedA,0); //Right fwd
majik 0:59f6f94baeb1 368 }
majik 0:59f6f94baeb1 369 else
majik 0:59f6f94baeb1 370 {
majik 0:59f6f94baeb1 371 motor_control(0,speedA,0); //Right fwd
majik 0:59f6f94baeb1 372 motor_control(1,speedB,0); //Left fwd
majik 0:59f6f94baeb1 373 }
majik 0:59f6f94baeb1 374 }
majik 0:59f6f94baeb1 375 void turn_right(int speedA, int speedB)
majik 0:59f6f94baeb1 376 {
majik 0:59f6f94baeb1 377 if (speedA > 0) //R fwd//
majik 0:59f6f94baeb1 378 {
majik 0:59f6f94baeb1 379 motor_control(0,speedA,0); //Right fwd
majik 0:59f6f94baeb1 380 }
majik 0:59f6f94baeb1 381 else
majik 0:59f6f94baeb1 382 {
majik 0:59f6f94baeb1 383 motor_control(0,speedA,1); //Right reverrse
majik 0:59f6f94baeb1 384 }
majik 0:59f6f94baeb1 385 if(speedB<0) //L rev
majik 0:59f6f94baeb1 386 {
majik 0:59f6f94baeb1 387 motor_control(1,speedB,1); //Left reverse
majik 0:59f6f94baeb1 388 }
majik 0:59f6f94baeb1 389 else
majik 0:59f6f94baeb1 390 motor_control(1,speedB,0);
majik 0:59f6f94baeb1 391
majik 0:59f6f94baeb1 392 }