motor.h

00001 float motor_speed[2] = {0,0}; //left, right (real exact current speed of each motor for the next raltime slot)  SI
00002 //float gy_old;
00003 //float vert;//ugol
00004 
00005 /*float max_speed = 1;
00006 float max_accel = 5;
00007 float max_eps = 20;*/
00008 
00009 DigitalOut  dir_left(PC_15);
00010 DigitalOut  dir_right(PA_4);
00011 DigitalOut  motor_enable(PA_0);//sleep inverted
00012 PwmOut step_left(PB_4);
00013 PwmOut step_right(PA_1);
00014 
00015 void geometric_navigation(){  
00016 
00017     current.speed = (motor_speed[1] + motor_speed[0])/2.0; 
00018     current.omega = (motor_speed[0] - motor_speed[1])/(2 * half_axis);  //rad/s          //MOVE TO SKOROST!!!!!
00019     current.azimuth += current.omega * t_step ;//rad
00020     if (current.azimuth > pi) current.azimuth -= 2*pi;
00021     if (current.azimuth < -pi) current.azimuth += 2*pi;
00022     current.x += current.speed * t_step * cos(current.azimuth);
00023     current.y += current.speed * t_step * sin(current.azimuth);
00024     current.path += current.speed * t_step;
00025     //pc.printf("%3.1f - %3.1f ; %3.1f - %3.1f ; %d\n",target_path,current_path,target_angle,current_angle,motor_busy); //be careful in interrupt
00026     }
00027 
00028 void set_motor_speed(float speed_left,float speed_right){
00029     motor_speed[0] = speed_left;
00030     motor_speed[1] = speed_right;    
00031 
00032     if (motor_speed[0] > max.speed*2) motor_speed[0] = max.speed*2;
00033     if (motor_speed[0] < -max.speed*2) motor_speed[0] = -max.speed*2;
00034     if (motor_speed[1] > max.speed*2) motor_speed[1] = max.speed*2;
00035     if (motor_speed[1] < -max.speed*2) motor_speed[1] = -max.speed*2;
00036 
00037     if (motor_speed[1] > 0) dir_right = 1;
00038     else dir_right = 0;
00039     if (motor_speed[0] > 0) dir_left = 0;
00040     else dir_left = 1;
00041     float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz
00042     float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz
00043     if (nu_l < 1) nu_l = 1;
00044     if (nu_r < 1) nu_r = 1;
00045     step_right.period_us (1e6/nu_r);
00046     step_right = 0.5;
00047     step_left.period_us (1e6/nu_l);
00048     step_left = 0.5; 
00049     geometric_navigation();   
00050 }
00051 
00052 void motor_init()
00053 {
00054     step_left = 0.5;
00055     step_right = 0.5;
00056     motor_enable = 1;
00057 }
00058 
00059