void checkendstage(int max, int min)
{
if((max-min)<30){ //if y fall in small range stop the car in 1 sec
motor_control(1,0,50);
motor_control(2,0,50);
_delay_ms(1000);
motor_control(1,0,0);
motor_control(2,0,0);
}
}
void PIT0_IRQHandler(void)
{ if(aa<1000) aa++;
delayms();
mpu6050_read();
angle_calculate();
angle_calculate1();
if (flag==2||flag==1)
set_gyro();
if (flag==3&&active==1)
{
set_gyro1_3();
set_gyro_3();
}
// oledplay();
if(flag==1)
{
motor_control();
SetMotorVoltage(0.05,0);
MotorSpeedOut();
}
if(qd==0)
{
motor_control();
SetMotorVoltage(0.05,0);
MotorSpeedOut();
}
if (flag==3&&active==1)
{
motor_control_3();
// SetMotorVoltage(0,0.5);
MotorSpeedOut_3();
}
if (flag==4)
{
motor_control_4();
SetMotorVoltage(0.05,0);
MotorSpeedOut();
}
if(flag==5)
{
set_gyro1();
motor_control1();
SetMotorVoltage(0,0.05);
MotorSpeedOut1();
angle_set1=16;
}
PIT_Flag_Clear(PIT0); //清中断标志位
}
/* Calculate backemf and save in g_pos
*/
void calculate_backemf (uint8_t motor)
{
int16_t backemf=0;
// Read backemf
adc_init(3);
backemf = read_adc(0x10);
if (backemf & 0x200)
{
backemf = (~(backemf) & (0x03FF))+1;
// backemf -= 512;
}
if (backemf < MOTOR_OFFSET)
backemf = 0;
g_pos[motor] += backemf;
if (g_mode[motor] != MOTOR_STOP)
debug_value(backemf,10);
//Compare voltage
if (((g_angle[motor] > 1)) && ( g_mode[motor] != MOTOR_STOP))
{
if ( g_pos[motor] >= (g_angle[motor])) // Check for position
{
motor_control(1, MOTOR_BRAKE);
delay(3);
g_mode[motor] = MOTOR_STOP;
uart_puts ("Voltage SUM");
debug_value(g_pos[motor],10);
g_pos[motor] =0;
}
}
}
// main acts as a cyclical task sequencer
void main(void)
{
OpenTimer0(TIMER_INT_OFF & T0_SOURCE_INT & T0_16BIT & T0_PS_1_64);
initialization(); // function from sumovore.c
// it sets up pwm (using timer2),
// IO pins, the ADC, the
// USART and the default
// threshold
printf("\n\rKwantlen APSC1299 simple curve follower -- with error codes\n\r");
ClrWdt(); // defined in <p18f4525.h>
threshold = 650u; // to change from default value
// uncomment and change to any unsigned int <1024u -- most usually <512u
while(1)
{
check_sensors(); // from sumovore.c
set_leds(); // function from sumovore.c
switch(SeeLine.B) // this is a switch based on the sensor values, SeeLine.B allows us to input sensor states as a 5 bit binary value
// it is already built into sumovore.c , so 0b11111 is all the sensors are on and 0b00000 is all sensors off
{
case 0b00100: motor_control();
break;
case 0b01110: motor_control();
break;
case 0: Robot_track_end();
break;
}
motor_control(); // function from motor_control.c
ClrWdt(); // defined in <p18f4525.h>
if(SeeLine.B==0)
{
}
if(lvd_flag_set()) LVtrap();
}
}
void encoder_callback() {
unsigned long long time = (unsigned long long)encoder->get_stamp_secs()*1000000 + (unsigned long long)encoder->get_stamp_nsecs()/1000;
TR.setWithEulers(3, 2,
.02, 0, .02,
0.0, 0.0, 0.0,
time);
TR.setWithEulers(2, 1,
0, 0, 0,
0, -encoder->val, 0,
time);
motor_control(); // Control on encoder reads sounds reasonable
// printf("I got some encoder values: %g!\n", encoder->val);
}
void start()
{
motor_control(1,0,100);
motor_control(2,0,100);
_delay_ms(2000);
motor_control(1,0,0);
motor_control(2,0,100);
_delay_ms(2000);
motor_control(1,0,100);
motor_control(2,0,100);
_delay_ms(4000);
motor_control(1,0,0);
motor_control(2,0,0);
}
/*int main()
{
LED_INIT();
uart_init(COM3, 115200);
//uart_interrupt_init(COM3, &listener);
tft_init(0, WHITE, BLACK, GREEN);
button_init();
ticks_init();
motor_init();
pneumatic_init();
int count = 0;
tft_init(0, WHITE, BLACK, GREEN);
linear_ccd_init();
adc_init();
while(true) {
if(count != get_ms_ticks()){
count = get_ms_ticks();
if(count % 40 == 0){
linear_ccd_clear();
linear_ccd_read();
linear_ccd_prints();
if(find_white_line() != 0) {
tft_prints(2, 1, "%3d", find_white_line());
tft_update();
}
if(find_white_line() < 50) {
motor_control(1,0,100);
}
else {
motor_control(1,0,0);
}
//tft_update();
}
}
}
return 0;
}*/
int main() {
tft_init(0,WHITE,BLACK,RED);
linear_ccd_init();
ticks_init();
GPIO_switch_init();
pneumatic_init();
button_init();
LED_INIT();
motor_init();
uart_init(COM3, 115200);
uart_interrupt_init(COM3,&listener);
while(true) {
//listener(const uint8_t byte);
tft_clear();
tft_prints(2,4,"okay la") ;
tft_update();
while(!read_button(1))
{ LED_ON(GPIOB, LED_M);
motor_control(1,1,100);
}
/*if(read_GPIO_switch(GPIOA,GPIO_Pin_9)==1&&!read_button(1))
{ tft_clear();
tft_prints(2,4,"grip");
tft_update();*/
/*pneumatic_control(GPIOB,GPIO_Pin_13,1);}
else
{ tft_clear();
tft_prints(2,4,"let it go~");
tft_update();
pneumatic_control(GPIOB,GPIO_Pin_13,0);}*/
int time = get_ms_ticks();
if(time%40==0)
{
linear_ccd_clear();
linear_ccd_read();
linear_ccd_prints();
}
//turnn(find_white_line());
}
}
/*************************************************
名称：calculate(float pitch, float roll, float yaw, u8 throttle, u8 rudder, u8 elevator, u8 aileron, float gx, float gy, float gz)
功能：系统pid运算
输入参数：
float pitch 当前俯仰角度
float roll 当前倾斜角度
float yaw 当前偏航角度
u8 throttle 油门给定值
u8 rudder 偏航给定值
u8 elevator 俯仰给定值
u8 aileron 倾斜给定值
float gx x轴角速度
float gy y轴角速度
float gz z轴角速度
输出参数：电机控制量
返回值： 无
**************************************************/
void calculate(float pitch, float roll, float yaw, u8 throttle, u8 rudder, u8 elevator, u8 aileron, float gx, float gy, float gz)
{
u16 thr = 0;
s16 pitch_out = 0, roll_out = 0, yaw_out = 0;
s16 m1 = 0, m2 = 0, m3 = 0, m4 = 0;
if(elevator > 0) // 后
{
if(elevator >= 0x80)
{
elevator -= 0x80;
pid_pitch.zero_err = elevator * 0.2;
}
else // 前
{
pid_pitch.zero_err = -(elevator * 0.2);
}
}
else
{
pid_pitch.zero_err = 0;
}
if(aileron > 0) // 右
{
if(aileron >= 0x80)
{
aileron -= 0x80;
pid_roll.zero_err = -(aileron * 0.2);
}
else // 左
{
pid_roll.zero_err = aileron * 0.2;
}
}
else
{
pid_roll.zero_err = 0;
}
if(rudder > 0) // 右偏
{
if(rudder >= 0x80)
{
rudder -= 0x80;
pid_yaw.zero_err = rudder * 20.0;
}
else // 左偏
{
pid_yaw.zero_err = -rudder * 20.0;
}
}
else
{
pid_yaw.zero_err = 0;
}
roll_out = (s16)pid_calculate(&pid_roll, roll, gx);
pitch_out = (s16)pid_calculate(&pid_pitch, pitch, gy);
yaw_out = (s16)pid_calculate(&pid_yaw, yaw, gz);
if(throttle > 0) // 油门
{
thr = throttle * 7.0;
m1 = thr - pitch_out + roll_out + yaw_out;
m2 = thr + pitch_out + roll_out - yaw_out;
m3 = thr + pitch_out - roll_out + yaw_out;
m4 = thr - pitch_out - roll_out - yaw_out;
}
else
{
m1 = 0;
m2 = 0;
m3 = 0;
m4 = 0;
pid_reset();
}
motor_control(m1, m2, m3, m4);
}
void main()
{
uint8_t sw1,sw2,i;
int16_t backemf[MOTOR_MAX];
uint16_t k,pwm,loop[MOTOR_MAX] = {0.0};
int8_t d1,d2;
uint8_t show[3] = {0,'f','b'};
init_board();
d1 = 0;
d2 = 0;
while (1)
{
sw1 = _7SEGMENT_SW1_IN_PORT & _7SEGMENT_SW1;
if (!sw1) // SW1 Press
{
d1++;
if (d1 > MOTOR_BACKWARD)
d1 = MOTOR_STOP;
for (k = 0; k < 500 ; k++) // Delay for key bounce
delay(6000);
g_mode[0] = d1;
g_angle[0] = 1; // Continuous move
g_motor_change[0] =1;
}
sw2 = _7SEGMENT_SW2_IN_PORT & _7SEGMENT_SW2;
if (!sw2) // SW2 Press
{
d2++;
if (d2 > MOTOR_BACKWARD)
d2 = MOTOR_STOP;
for (k = 0; k < 500 ; k++) // Delay for key bounce
delay(6000);
g_mode[1] = d2;
g_angle[1] = 1; // Continuous move
g_motor_change[1] =1;
}
// Command decode
if ( g_cmd_decode)
{
// Decode command
cmd_decode ();
g_cmd_decode = 0;
loop[g_motor] = 0;
}
for (i = 0 ; i < MOTOR_MAX ; i++)
{
// If angle > 1 loop start
if (g_angle[i] > 1)
{
loop[i]++;
if ( loop[i] == (g_angle[i]*50))
{
g_mode[i] = MOTOR_STOP;
g_motor_change[i] = 1;
loop[i] =0;
}
}
if (g_motor_change[i] ) // Motor command change
{
// Motor contol section
// Set speed
pwm = (uint16_t)(g_speed[i] * 100); // Start from + speed*2^4
if (i == 0 )
OCR1A = pwm;
if (i == 1)
OCR1B = pwm;
// Set direction
motor_control(i+1, g_mode[i]);
g_motor_change[i] = 0;
}
// Read ADC value
// backemf[i] = read_adc(
// 7 Segments display
d7segment_display(show[g_mode[i]],i+1);
// delay(10000);
}
}
}
void listener(const uint8_t byte) {
switch(byte) {
case 'w':
tft_prints(1,1,"Yay Forward LA");
tft_update();
motor_control(1,1,100);
motor_control(2,1,100);
_delay_ms(delay_time);
motor_control(1, 1, 0);
motor_control(2, 1, 0);
break;
case 'a':
tft_prints(1,1,"Yay Left LA");
tft_update();
motor_control(1, 1, 0);
motor_control(2, 1, 100);
_delay_ms(delay_time);
motor_control(1, 1, 0);
motor_control(2, 1, 0);
break;
case 'd':
tft_prints(1, 1, "Yay Right LA");
tft_update();
motor_control(1,1,100);
motor_control(2,1,0);
_delay_ms(delay_time);
motor_control(1, 1, 0);
motor_control(2, 1, 0);
break;
case 's':
tft_prints(1, 1, "Yay Stop La");
tft_update();
motor_control(1, 1, 0);
motor_control(2, 1, 0);
//stop_while = 0;
break;
case 'b':
stop_while = 1;
break;
default:
tft_prints(1, 1, "default la");
break;
}
}
long redrobd_ctrl_thread::cyclic_execute(void)
{
try {
// Check if shutdown was selected (DIP-switch)
if ( (!m_shutdown_select) && (m_hw_cfg_auto->select_shutdown()) ) {
redrobd_log_writeln(get_name() + " : Shutdown selected");
// Send signal to main process
if (send_signal_self(SIG_TERMINATE_DAEMON) != DAEMON_SUCCESS) {
THROW_EXP(REDROBD_LINUX_ERROR, REDROBD_SIGNAL_OPERATION_FAILED,
"Error sending shutdown signal for thread %s",
get_name().c_str());
}
m_shutdown_select = true; // Only signal once
}
// Check battery voltage
if ( !battery_voltage_ok() ) {
redrobd_led_bat_low(true); // Turn status LED on
}
else {
redrobd_led_bat_low(false); // Turn status LED off
}
// Check system stats
check_system_stats();
// Check state and status of created threads
check_thread_run_status();
// Check remote control steering
uint16_t steering = get_remote_steering();
// Do motor control
switch (steering) {
case REDROBD_RC_STEER_NONE:
motor_control(REDROBD_MC_NONE);
break;
case REDROBD_RC_STEER_FORWARD:
if (m_verbose) {
redrobd_log_writeln(get_name() + " : steer forward");
}
motor_control(REDROBD_MC_FORWARD);
break;
case REDROBD_RC_STEER_REVERSE:
if (m_verbose) {
redrobd_log_writeln(get_name() + " : steer reverse");
}
motor_control(REDROBD_MC_REVERSE);
break;
case REDROBD_RC_STEER_RIGHT:
if (m_verbose) {
redrobd_log_writeln(get_name() + " : steer right");
}
motor_control(REDROBD_MC_RIGHT);
break;
case REDROBD_RC_STEER_LEFT:
if (m_verbose) {
redrobd_log_writeln(get_name() + " : steer left");
}
motor_control(REDROBD_MC_LEFT);
break;
default:
// All other steerings are ignored for now
ostringstream oss_msg;
oss_msg << get_name()
<< " : Got undefined steering = 0x"
<< hex << setw(4) << setfill('0') << (unsigned)steering;
redrobd_log_writeln(oss_msg.str());
oss_msg.str("");
motor_control(REDROBD_MC_STOP);
}
// Check remote control camera code
uint16_t camera_code = m_rc_net_auto->get_camera_code();
// Do camera control
switch (camera_code) {
case REDROBD_RC_CAMERA_NONE:
camera_control(REDROBD_CC_NONE);
break;
case REDROBD_RC_CAMERA_STOP_STREAM:
camera_control(REDROBD_CC_STOP_STREAM);
break;
case REDROBD_RC_CAMERA_START_STREAM:
camera_control(REDROBD_CC_START_STREAM);
break;
default:
// All other camera codes are ignored for now
ostringstream oss_msg;
oss_msg << get_name()
<< " : Got undefined camera code = 0x"
<< hex << setw(4) << setfill('0') << (unsigned)steering;
redrobd_log_writeln(oss_msg.str());
oss_msg.str("");
}
return THREAD_SUCCESS;
}
catch (excep &exp) {
syslog_error(redrobd_error_syslog_string(exp).c_str());
return THREAD_INTERNAL_ERROR;
}
catch (...) {
syslog_error("redrobd_ctrl_thread::cyclic_execute->Unexpected exception");
return THREAD_INTERNAL_ERROR;
}
}
void turn(int x){
if (x>0 && x<20){
motor_control(1, 0, 20);
motor_control(2, 0, 165);
}else if(x>20 && x<40){
motor_control(1, 0, 20);
motor_control(2, 0, 140);
}else if(x>40 && x<54){
motor_control(1, 0, 50);
motor_control(2, 0, 110);
}
if(x>74 && x<100){
motor_control(1, 0, 110); //left motor
motor_control(2, 0, 50);
}else if(x<100 && x>120){
motor_control(1,0, 140);
motor_control(2,0, 20);
}else if (x>120){
motor_control(1,0,165);
motor_control(2,0, 20);
}
}
void turnn(int x){
if (x>0 && x<20){
motor_control(1, 0, 0); //left motor
motor_control(2, 0, 100); //right motor
tft_prints(5,5,"Turn left");
tft_update();
}else if(x>20 && x<40){
motor_control(1, 0, 0);
motor_control(2, 0, 75);
tft_prints(5,5,"Turn left");
tft_update();
}else if(x>40 && x<54){
motor_control(1, 0, 0);
motor_control(2, 0, 70);
tft_prints(5,5,"Turn left");
tft_update();
}
if(x>74 && x<100){
motor_control(1, 0, 100); //left motor
motor_control(2, 0, 0);
tft_prints(5,5,"Turn right");
tft_update();
}else if(x<100 && x>120){
motor_control(1,0, 75);
motor_control(2,0, 0);
tft_prints(5,5,"Turn right");
tft_update();
}else if (x>120){
motor_control(1,0,70);
motor_control(2,0, 0);
tft_prints(5,5,"Turn right");
tft_update();
}
}
