Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motion.h@4:904b737ef08a, 2017-05-14 (annotated)

Committer:
Stas285
Date:
Sun May 14 11:28:29 2017 +0000
Revision:
4:904b737ef08a
Parent:
1:e2a6e523bf1f
Child:
6:6e89cdc3db92
+Gyro(draft)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 0:e9488589a8ee 1 //float current_speed;//cm/s
Stas285 0:e9488589a8ee 2 float half_width = 8.5;
Stas285 0:e9488589a8ee 3 float radius = 10;
Stas285 0:e9488589a8ee 4 float speed = 100;//cm/s
Stas285 4:904b737ef08a 5 float current_x, current_y;
Stas285 4:904b737ef08a 6 float current_path, target_path;//cm
Stas285 4:904b737ef08a 7 float current_angle, target_angle;//deg
Stas285 0:e9488589a8ee 8 float old_path = 0;
Stas285 0:e9488589a8ee 9 float old_angle = 0;
Stas285 0:e9488589a8ee 10 bool volatile motor_busy;
Stas285 4:904b737ef08a 11 bool path_dir, angle_dir;
Stas285 0:e9488589a8ee 12 bool angle_task;//1 - angle, 0 - path
Stas285 0:e9488589a8ee 13 bool stop_flag = 0;
Stas285 1:e2a6e523bf1f 14 bool infinite_flag = 0;
Stas285 0:e9488589a8ee 15
Stas285 0:e9488589a8ee 16 //Timer trap_timer;
Stas285 0:e9488589a8ee 17
Stas285 0:e9488589a8ee 18 void motion_init(){
Stas285 0:e9488589a8ee 19 //trap_timer.start();
Stas285 0:e9488589a8ee 20 }
Stas285 0:e9488589a8ee 21
Stas285 0:e9488589a8ee 22 void go(int cm, bool brake = 1){
Stas285 0:e9488589a8ee 23 if(cm != 0) {
Stas285 1:e2a6e523bf1f 24 stop_flag = 0; infinite_flag = 0;
Stas285 0:e9488589a8ee 25 motor_enable = 1;
Stas285 0:e9488589a8ee 26 target_path =old_path + cm; old_path = target_path;
Stas285 0:e9488589a8ee 27 if(cm > 0) path_dir = 1; else path_dir = 0;
Stas285 0:e9488589a8ee 28 angle_task = 0;
Stas285 0:e9488589a8ee 29 motor_busy = 1;
Stas285 0:e9488589a8ee 30 while(motor_busy == 1){proc_counter++;}
Stas285 0:e9488589a8ee 31 }
Stas285 0:e9488589a8ee 32 }
Stas285 0:e9488589a8ee 33
Stas285 0:e9488589a8ee 34 void turn(int deg, bool brake = 1){
Stas285 0:e9488589a8ee 35 if(deg != 0) {
Stas285 1:e2a6e523bf1f 36 stop_flag = 0; infinite_flag = 0;
Stas285 0:e9488589a8ee 37 motor_enable = 1;
Stas285 0:e9488589a8ee 38 target_angle = old_angle + deg; old_angle = target_angle;
Stas285 0:e9488589a8ee 39 if(deg > 0) angle_dir = 1; else angle_dir = 0;
Stas285 0:e9488589a8ee 40 angle_task = 1;
Stas285 0:e9488589a8ee 41 motor_busy = 1;
Stas285 0:e9488589a8ee 42 while(motor_busy == 1){proc_counter++;}
Stas285 0:e9488589a8ee 43 old_path = current_path;
Stas285 0:e9488589a8ee 44 }
Stas285 0:e9488589a8ee 45 }
Stas285 0:e9488589a8ee 46
Stas285 0:e9488589a8ee 47 void stop(){
Stas285 0:e9488589a8ee 48 motor_enable = 1;
Stas285 0:e9488589a8ee 49 stop_flag = 1;
Stas285 0:e9488589a8ee 50 motor_busy = 1;
Stas285 0:e9488589a8ee 51 while(motor_busy == 1){}
Stas285 4:904b737ef08a 52 //motor_enable = 0;
Stas285 0:e9488589a8ee 53 }
Stas285 0:e9488589a8ee 54
Stas285 0:e9488589a8ee 55 void dance(float dance_speed,float dance_accel){
Stas285 0:e9488589a8ee 56 speed = dance_speed;
Stas285 0:e9488589a8ee 57 accel=dance_accel;
Stas285 0:e9488589a8ee 58 radius=20;
Stas285 0:e9488589a8ee 59
Stas285 0:e9488589a8ee 60 go(25);
Stas285 0:e9488589a8ee 61 go(-50);
Stas285 0:e9488589a8ee 62 go(25);
Stas285 0:e9488589a8ee 63 turn(90);
Stas285 0:e9488589a8ee 64 speed = -dance_speed;
Stas285 0:e9488589a8ee 65 turn(-180);
Stas285 0:e9488589a8ee 66 speed = dance_speed;
Stas285 0:e9488589a8ee 67 turn(90);
Stas285 0:e9488589a8ee 68 turn(-360);
Stas285 0:e9488589a8ee 69 turn(360);
Stas285 0:e9488589a8ee 70
Stas285 0:e9488589a8ee 71 go(20);
Stas285 0:e9488589a8ee 72 speed = -dance_speed;
Stas285 0:e9488589a8ee 73 turn(-90);
Stas285 0:e9488589a8ee 74 speed = dance_speed;
Stas285 0:e9488589a8ee 75 turn(-90);
Stas285 0:e9488589a8ee 76 speed = -dance_speed;
Stas285 0:e9488589a8ee 77 turn(-90);
Stas285 0:e9488589a8ee 78 speed = dance_speed;
Stas285 0:e9488589a8ee 79 turn(-90);
Stas285 0:e9488589a8ee 80 go(-20);
Stas285 0:e9488589a8ee 81
Stas285 0:e9488589a8ee 82 radius =0;
Stas285 0:e9488589a8ee 83 turn(720);
Stas285 0:e9488589a8ee 84 turn(-720);
Stas285 0:e9488589a8ee 85 }