Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motion.h@10:5bdd3dfd5f59, 2018-09-05 (annotated)

Committer:
dima285
Date:
Wed Sep 05 18:25:54 2018 +0000
Revision:
10:5bdd3dfd5f59
Parent:
9:8f98b1c277a4
Child:
12:721a9ea55e91
Stable 05.09.2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 6:6e89cdc3db92 1 //3 main parameters of the movement:
Stas285 6:6e89cdc3db92 2 //float accel (defined in motor.h)
dima285 9:8f98b1c277a4 3 float speed = 1;//cm/s
Stas285 6:6e89cdc3db92 4 float radius = 10;
Stas285 6:6e89cdc3db92 5
dima285 10:5bdd3dfd5f59 6 float current_speed;
dima285 10:5bdd3dfd5f59 7 //float half_width = 8.5;
Stas285 4:904b737ef08a 8 float current_x, current_y;
dima285 10:5bdd3dfd5f59 9 float current_path, target_path;
Stas285 4:904b737ef08a 10 float current_angle, target_angle;//deg
Stas285 0:e9488589a8ee 11 float old_path = 0;
Stas285 0:e9488589a8ee 12 float old_angle = 0;
Stas285 0:e9488589a8ee 13 bool volatile motor_busy;
Stas285 4:904b737ef08a 14 bool path_dir, angle_dir;
Stas285 0:e9488589a8ee 15 bool angle_task;//1 - angle, 0 - path
Stas285 0:e9488589a8ee 16 bool stop_flag = 0;
Stas285 1:e2a6e523bf1f 17 bool infinite_flag = 0;
Stas285 0:e9488589a8ee 18
Stas285 0:e9488589a8ee 19 //Timer trap_timer;
Stas285 0:e9488589a8ee 20
Stas285 0:e9488589a8ee 21 void motion_init(){
Stas285 0:e9488589a8ee 22 //trap_timer.start();
Stas285 0:e9488589a8ee 23 }
Stas285 0:e9488589a8ee 24
dima285 10:5bdd3dfd5f59 25 void go(float distance_m, bool brake = 1){
dima285 10:5bdd3dfd5f59 26 //if(cm != 0) {
dima285 10:5bdd3dfd5f59 27 //stop_flag = 0; infinite_flag = 0;
dima285 10:5bdd3dfd5f59 28 //motor_enable = 1;
dima285 10:5bdd3dfd5f59 29 target_path = current_path + distance_m;
dima285 10:5bdd3dfd5f59 30 wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed);
dima285 10:5bdd3dfd5f59 31 while(abs(target_path - current_path) > 0.01){gyro_process();wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
dima285 10:5bdd3dfd5f59 32 //if(cm > 0) path_dir = 1; else path_dir = 0;
dima285 10:5bdd3dfd5f59 33 //angle_task = 0;
dima285 10:5bdd3dfd5f59 34 //motor_busy = 1;
dima285 10:5bdd3dfd5f59 35 //while(motor_busy == 1){proc_counter++;}
dima285 10:5bdd3dfd5f59 36 //}
Stas285 0:e9488589a8ee 37 }
dima285 8:891e4f54e9e2 38
dima285 8:891e4f54e9e2 39 void go_no_wait(int cm, bool brake = 1){
dima285 8:891e4f54e9e2 40 if(cm != 0) {
dima285 8:891e4f54e9e2 41 stop_flag = 0; infinite_flag = 0;
dima285 8:891e4f54e9e2 42 motor_enable = 1;
dima285 8:891e4f54e9e2 43 target_path =old_path + cm; old_path = target_path;
dima285 8:891e4f54e9e2 44 if(cm > 0) path_dir = 1; else path_dir = 0;
dima285 8:891e4f54e9e2 45 angle_task = 0;
dima285 8:891e4f54e9e2 46 motor_busy = 1;
dima285 8:891e4f54e9e2 47 //while(motor_busy == 1){proc_counter++;}
dima285 8:891e4f54e9e2 48 }
dima285 8:891e4f54e9e2 49 }
Stas285 0:e9488589a8ee 50
Stas285 0:e9488589a8ee 51 void turn(int deg, bool brake = 1){
Stas285 0:e9488589a8ee 52 if(deg != 0) {
Stas285 1:e2a6e523bf1f 53 stop_flag = 0; infinite_flag = 0;
Stas285 0:e9488589a8ee 54 motor_enable = 1;
Stas285 0:e9488589a8ee 55 target_angle = old_angle + deg; old_angle = target_angle;
Stas285 0:e9488589a8ee 56 if(deg > 0) angle_dir = 1; else angle_dir = 0;
Stas285 0:e9488589a8ee 57 angle_task = 1;
Stas285 0:e9488589a8ee 58 motor_busy = 1;
Stas285 0:e9488589a8ee 59 while(motor_busy == 1){proc_counter++;}
Stas285 0:e9488589a8ee 60 old_path = current_path;
Stas285 0:e9488589a8ee 61 }
Stas285 0:e9488589a8ee 62 }
Stas285 0:e9488589a8ee 63
Stas285 0:e9488589a8ee 64 void stop(){
Stas285 0:e9488589a8ee 65 motor_enable = 1;
Stas285 0:e9488589a8ee 66 stop_flag = 1;
Stas285 0:e9488589a8ee 67 motor_busy = 1;
Stas285 0:e9488589a8ee 68 while(motor_busy == 1){}
Stas285 6:6e89cdc3db92 69 target_path = current_path;
Stas285 4:904b737ef08a 70 //motor_enable = 0;
Stas285 0:e9488589a8ee 71 }
Stas285 0:e9488589a8ee 72
Stas285 0:e9488589a8ee 73 void dance(float dance_speed,float dance_accel){
Stas285 0:e9488589a8ee 74 speed = dance_speed;
Stas285 0:e9488589a8ee 75 accel=dance_accel;
Stas285 0:e9488589a8ee 76 radius=20;
Stas285 0:e9488589a8ee 77
Stas285 0:e9488589a8ee 78 go(25);
Stas285 0:e9488589a8ee 79 go(-50);
Stas285 0:e9488589a8ee 80 go(25);
Stas285 0:e9488589a8ee 81 turn(90);
Stas285 0:e9488589a8ee 82 speed = -dance_speed;
Stas285 0:e9488589a8ee 83 turn(-180);
Stas285 0:e9488589a8ee 84 speed = dance_speed;
Stas285 0:e9488589a8ee 85 turn(90);
Stas285 0:e9488589a8ee 86 turn(-360);
Stas285 0:e9488589a8ee 87 turn(360);
Stas285 0:e9488589a8ee 88
Stas285 0:e9488589a8ee 89 go(20);
Stas285 0:e9488589a8ee 90 speed = -dance_speed;
Stas285 0:e9488589a8ee 91 turn(-90);
Stas285 0:e9488589a8ee 92 speed = dance_speed;
Stas285 0:e9488589a8ee 93 turn(-90);
Stas285 0:e9488589a8ee 94 speed = -dance_speed;
Stas285 0:e9488589a8ee 95 turn(-90);
Stas285 0:e9488589a8ee 96 speed = dance_speed;
Stas285 0:e9488589a8ee 97 turn(-90);
Stas285 0:e9488589a8ee 98 go(-20);
Stas285 0:e9488589a8ee 99
Stas285 0:e9488589a8ee 100 radius =0;
Stas285 0:e9488589a8ee 101 turn(720);
Stas285 0:e9488589a8ee 102 turn(-720);
dima285 10:5bdd3dfd5f59 103 }
dima285 10:5bdd3dfd5f59 104
dima285 10:5bdd3dfd5f59 105