static int set_motor_speed(shared_data_t *data, int speed_m1, int speed_m2)
{
/* The adjustment is used if the speed is the same for both motors. */
if (speed_m1 == speed_m2){
if (speed_m1 < 0)
speed_m1 += motor_1.adjust;
else
speed_m1 -= motor_1.adjust;
if (speed_m2 < 0)
speed_m2 += motor_2.adjust;
else
speed_m2 -= motor_2.adjust;
}
syslog(LOG_DEBUG, "Real speed: %i %i\n", speed_m1, speed_m2);
/* Switch direction */
if ((motor_1.cur_speed < 0 && speed_m1 > 0) ||
(motor_1.cur_speed > 0 && speed_m1 < 0))
motor_switch_direction(motor_1);
if ((motor_2.cur_speed < 0 && speed_m2 > 0) ||
(motor_2.cur_speed > 0 && speed_m2 < 0))
motor_switch_direction(motor_2);
/* Update speed */
motor_speed(data, &motor_1, speed_m1);
motor_speed(data, &motor_2, speed_m2);
return 0;
}
uint16_t pid_find_max_encoder_value(void){
uint16_t enc_val = 0, prev_enc_val = -1;
motor_speed(75);
//Stop at left end
while(enc_val != prev_enc_val){
printf("In while\n");
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
printf("Out of while\n");
motor_speed(0);
motor_encoder_reset();
//Stop at right end and set max
motor_speed(-75);
enc_val = 0, prev_enc_val = -1;
while(enc_val != prev_enc_val){
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
motor_speed(0);
//Max value of encoder.
return enc_val;
}
//0 is to the right, and position increases leftward
void motor_crude_controller(uint16_t current_position, uint16_t reference){
//if requested position is out of reach
if(reference > max_left){
motor_crude_controller(max_left, reference);
}
//if we're close to ref
if(abs(current_position - reference) < 300){
motor_speed(0);
return;
}
//too far left
if(current_position > reference){
motor_direction(right);
motor_speed(100);
}
//too far right
else{
motor_direction(left);
motor_speed(100);
}
}
//MOTOR SPEED NEEDS TO BE TUNED FOR INDIVIDUAL GAME BOARD
void motor_controller_calibrate_by_reset(){
uint8_t speed = 65;
motor_init();
uint16_t position;
uint16_t prev_position;
motor_speed(speed);
motor_direction(right);
_delay_ms(150);
position = motor_encoder_read();
//go right until stopped, then set encoder to zero
while(position != prev_position){
position = motor_encoder_read();
_delay_ms(100);
prev_position = position;
position = motor_encoder_read();
printf("position: %u\tPrev: %u\n", position, prev_position);
}
motor_encoder_reset();
motor_speed(speed);
motor_direction(left);
_delay_ms(150);
//go left until stopped, set max left to current position
do{
position = motor_encoder_read();
_delay_ms(100);
prev_position = position;
position = motor_encoder_read();
printf("position: %d\n", position);
} while(position != prev_position);
max_left = position;
motor_speed(0);
motor_speed(speed);
motor_direction(right);
_delay_ms(100);
while(motor_encoder_read() > 4500){
_delay_ms(100);
}
}
int main(void)
{
int status;
memset(RX_BUF,0,4);
led_init();
nRF24L01_init();
motor_init(); //电机初始化
QuadCopter_init(&QuadCopter);
uart_init(115200);
status = nRF_Check(); /*检测NRF模块与MCU的连接*/
if(status == SUCCESS) /*判断连接状态*/
uart_printf("\r\n NRF与MCU连接成功！\r\n");
else
uart_printf("\r\n NRF与MCU连接失败，请重新检查接线。\r\n");
nRF_RX_Mode();
LED_OFF;
while(1)
{
nRF_RX_Mode();
nRF_Rx_Dat(RX_BUF);
status=RX_BUF[2];
switch(status)
{
case Q_ON:
LED_ON;
QuadCopter.Status=Q_ON;
QuadCopter.BaseSpeed=300;
motor_speed(QuadCopter.BaseSpeed,QuadCopter.BaseSpeed,QuadCopter.BaseSpeed,QuadCopter.BaseSpeed ) ;
break;
case Q_UP:
QuadCopter_up(&QuadCopter);
break;
case Q_DOWN:
QuadCopter_down(&QuadCopter);
break;
case Q_OFF:
LED_OFF;
QuadCopter.Status=Q_OFF;
motor_speed(0,0,0,0 ) ;
break;
default:
break;
}
}
}
/**
* \brief Get the current focuser state
*
* The GET request returns a single byte containing the state of all
* the output ports.
*/
void process_get() {
Endpoint_ClearSETUP();
unsigned short v[3];
v[0] = motor_current();
v[1] = motor_target();
v[2] = motor_speed();
Endpoint_Write_Control_Stream_LE((void *)v, 6);
Endpoint_ClearOUT();
}
//negative speed to the right to make transition to encoder values easier
void motor_speed_direction(int16_t speed){
if(speed < 0){
motor_direction(right);
}
else{
motor_direction(left);
}
motor_speed(abs(speed));
}
void motor_speed_direction_cap(int16_t speed, uint8_t cap){
if(speed < 0){
motor_direction(right);
}
else{
motor_direction(left);
}
if(abs(speed) > cap){
motor_speed(abs(cap));
}
}
uint8_t motor_speed_controller(uint8_t speed){
uint8_t tilt = abs(input_joystick_x() - 127);
if((tilt < 70) && tilt > 30){
motor_speed(90);
}
else if(tilt >= 70){
motor_speed(130);
}
else{
motor_speed(0);
}
if(input_joystick_x() > 127){
motor_speed(right);
}
else{
motor_speed(left);
}
}
// Initializes the motor in port C
void motor_init() {
max520_init(MAX520_TWI_ADDRESS);
DDRC |= (1<<PC3) | (1<<PC4) | (1<<PC5) | (1<<PC6) | (1<<PC7);
DDRK = 0;
motor_output_enable(ENABLE);
motor_encoder_reset();
motor_encoder_select_byte(ENCODER_HIGH);
motor_speed(0);
motor_enable(ENABLE);
}
uint16_t controller_init(){
motor_speed(75);
uint16_t enc_val = 0, prev_enc_val = -1;
while(enc_val != prev_enc_val){
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
motor_speed(0);
motor_encoder_reset();
motor_speed(-75);
enc_val = 0, prev_enc_val = -1;
while(enc_val != prev_enc_val){
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
motor_speed(0);
//Find max value of encoder.
return enc_val;
}
void OnButtonRTClicked(WM_MESSAGE * pMsg)
{
motor_speed(0);
if((get_data.e_work_state != START_TEST)&&(get_data.e_work_state != GET_V1))
{
if(ttpars.dir == 1)
{
BUTTON_SetText(WM_GetDialogItem(pMsg->hWin,GUI_ID_BUTTON_TRT),"反转");
ttpars.dir = 2;
}else {
BUTTON_SetText(WM_GetDialogItem(pMsg->hWin,GUI_ID_BUTTON_TRT),"正转");
ttpars.dir = 1;
}
motor_dir(ttpars.dir);
}
}
