Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motion.h@8:891e4f54e9e2, 2017-08-18 (annotated)

Committer:
dima285
Date:
Fri Aug 18 08:53:15 2017 +0000
Revision:
8:891e4f54e9e2
Parent:
6:6e89cdc3db92
Child:
9:8f98b1c277a4
jstrjm66MRRRRRRRRRRth

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