Don't ad-block us - support your favorite websites. We have safe, unobstrusive, robotics related ads that you actually want to see - see here for more.
0 Members and 1 Guest are viewing this topic.
#include <avr/io.h>#include <avr/interrupt.h>#include <avr/pgmspace.h>#include <stdlib.h>#include <ctype.h>#include <stdio.h>#include <math.h>//マクロ#define cbi(addr,bit) (addr &= ~(1<<bit))#define sbi(addr,bit) (addr |= (1<<bit))#define RTC_COMPARE 50000#define PRP_STICK_R_X 5 #define PRP_STICK_R_Y 3#define PRP_STICK_L_X 2#define PRP_STICK_L_Y 4#define PRP_SWITCH 1#define PRP_VOLUME 0#define PRP_STICK_R_X_CENTER 237 #define PRP_STICK_R_Y_CENTER 237#define PRP_STICK_L_X_CENTER 237#define PRP_STICK_L_Y_CENTER 237#define PRP_STICK_R_X_PLUS 301 #define PRP_STICK_R_Y_PLUS 173#define PRP_STICK_L_X_PLUS 302#define PRP_STICK_L_Y_PLUS 174#define PRP_STICK_R_X_MINUS 172#define PRP_STICK_R_Y_MINUS 302#define PRP_STICK_L_X_MINUS 172#define PRP_STICK_L_Y_MINUS 301#define MEDIAN_FILTER_ELEMENTS 4#define MOTOR_CH0_BRAKE 0x03 #define MOTOR_CH0_CW 0x01 #define MOTOR_CH0_CCW 0x02 #define MOTOR_CH0_FREE 0x00 #define MOTOR_CH1_BRAKE 0x30 #define MOTOR_CH1_CW 0x10 #define MOTOR_CH1_CCW 0x20 #define MOTOR_CH1_FREE 0x00 #define MOTOR_CH2_BRAKE 0x0c #define MOTOR_CH2_CW 0x04 #define MOTOR_CH2_CCW 0x08 #define MOTOR_CH2_FREE 0x00 #define MOTOR_CH0 0 #define MOTOR_CH1 1 #define MOTOR_CH2 2 //構造体typedef struct time_t{ uint8_t second; uint8_t accumulator_byte; uint8_t minute; uint8_t hour; uint16_t m_second; uint32_t accumulator;}time_t;typedef struct propo_pulse_t{ uint16_t old_time; uint16_t def_time; }propo_pulse_t;typedef struct motor_state_t{ uint8_t level; uint8_t direction; uint8_t level_buffer; uint8_t direction_buffer;;}motor_state_t;typedef struct encoder_state_t{ uint8_t accumulator_byte_def; uint16_t counter_def;}encoder_state_t;//グローバル変数volatile time_t time;volatile propo_pulse_t propo_pulse[6];volatile uint16_t g_debug;volatile motor_state_t motor_state[3];volatile encoder_state_t encoder_state[3];volatile uint8_t system_enable;//プロトタイプvoid SCI_open(uint16_t bps_value);void SCI_write(uint8_t tx);uint8_t SCI_read(void);void SCI_transmit_string(char *string);void wait_msec(uint16_t term);char *sitoa(uint16_t data,char *s);int16_t prp_get_pulse_width(uint8_t channel);void set_motor_power(uint8_t channel,int16_t level);//uint16_t get_motor_speed(uint8_t channel);//回転数を取得//回転方向は分からない#define get_motor_speed(channel) (uint16_t)(937500 / ((uint32_t)encoder_state[channel].counter_def + (uint32_t)encoder_state[channel].accumulator_byte_def * RTC_COMPARE))void motor_speed_control(int16_t speed);//cal 144//添え字とプロポの各チャネルの対応//0 ch6 右上のフラップ//1 ch5 左上のギア//2 左スティックのx//3 右スティックy//4 左スティックy//5 右スティックx//パルス幅データは生の値を128で除した値を使う//各モータチャネルとポートの対応//motor ch0 マイコンより PB0,PB1//motor ch1 真中 PD4,PD5//motor ch2 一番外側 PD2,PD3////ch0 右舷モータ プロポRY//ch1 左舷モータ プロポLY//ch2 上昇モータ プロポRX//ドライバファンクション// L L FREE// L R or R L CW/CCW// H H Brake//タイマ2オーバーフロー割り込み//ソフトウエアPWMの処理//面倒だからFreePWMでいいやISR (TIMER2_OVF_vect){ static uint8_t base_counter; if(system_enable == 0){ PORTB &= 0xFC; //全OFF PORTD &= 0xC3; return; } --base_counter; if(base_counter == 0){ PORTB &= 0xFC; //全OFF PORTD &= 0xC3; motor_state[MOTOR_CH0].level_buffer = motor_state[MOTOR_CH0].level; //値更新 motor_state[MOTOR_CH1].level_buffer = motor_state[MOTOR_CH1].level; motor_state[MOTOR_CH2].level_buffer = motor_state[MOTOR_CH2].level; motor_state[MOTOR_CH0].direction_buffer = motor_state[MOTOR_CH0].direction; motor_state[MOTOR_CH1].direction_buffer = motor_state[MOTOR_CH1].direction; motor_state[MOTOR_CH2].direction_buffer = motor_state[MOTOR_CH2].direction; }else{ if(base_counter == motor_state[MOTOR_CH0].level_buffer){ PORTB = (PORTB & 0xFC) | motor_state[MOTOR_CH0].direction_buffer; } if(base_counter == motor_state[MOTOR_CH1].level_buffer){ PORTD = (PORTD & 0xCF) | motor_state[MOTOR_CH1].direction_buffer; } if(base_counter == motor_state[MOTOR_CH2].level_buffer){ PORTD = (PORTD & 0xF3) | motor_state[MOTOR_CH2].direction_buffer; } }}#define P_GAIN 13#define I_GAIN 2#define CONTROL_VALUE_SCALE 2000//回転数制御をPIコントローラで行う//制御周期50msec//回転数の指定はrpmで行う//回転数の値の正負で正転逆転を指定するvoid motor_speed_control(int16_t speed){ uint16_t feed_back_speed; int32_t error; int32_t control_value; static int32_t error_old = 0; static int32_t control_value_old = 0; int32_t buf; feed_back_speed = get_motor_speed(MOTOR_CH0); error = speed - feed_back_speed; control_value = P_GAIN * error + I_GAIN * (error + error_old); control_value /= CONTROL_VALUE_SCALE; buf = control_value; //if( ((control_value > 0) && (speed < 0)) || ((control_value < 0) && (speed > 0)) ){ //control_value = 0; //} if(buf < 0){ buf = 0; } control_value += control_value_old; set_motor_power(MOTOR_CH0,control_value); error_old = error; control_value_old = control_value;}int main(void){ char tx_buffer[80]; int16_t pulse_buffer[6]; volatile double theta; int16_t r; int16_t angle_int; int16_t power_r; int16_t power_l; //ポートの設定 //DDRxが1で出力 DDRB = 0x03; DDRD = 0x3C; //入力のときにPORTを1にするとプルアップが有効になる PORTB = 0xff; PORTC = 0xff; PORTD = 0xff; //PCINT8-13を使ってパルスデコード //PCINT22,23はそれぞれmotor ch0,1のエンコーダ入力 //PCINT2はmotor ch2のエンコーダ入力 PCICR = 0x07; //PCINT8-14のピン変化割り込みを許可 PCMSK1 = 0x3f; //PCINT8-13のみ割り込み有効 PCMSK0 = 0x04; PCMSK2 = 0xC0; //タイマ0は超音波信号40KHzの生成に使用 //16ビットタイマを使ってリアルタイムクロックを動かす //タイマ1の設定 //プリスケーラ無し //コンペア周波数 400Hz TCCR1A = 0x04; //比較一致タイマ コンペアレジスタOCR1A TCCR1B = 0x09; //PS1 CTC ps1からps64に変更2116 OCR1A = RTC_COMPARE-1; TIMSK1 = 0x02; //コンペア割り込み許可 //タイマ2の設定 //オーバーフロー割り込みをかける //割り込み周波数78.125kHz TCCR2A = 0x00; //CTC動作 TCCR2B = 0x01; //プリスケーラ1/1 OCR2A = 250-1; //コンペア周期設定 TIMSK2 = 0x01; //コンペアA割り込み許可 sei(); //全割り込み許可 SCI_open(10); motor_state[0].direction = MOTOR_CH0_CW; motor_state[0].level = 125; motor_state[1].direction = MOTOR_CH1_CW; motor_state[1].level = 0; motor_state[2].direction = MOTOR_CH2_CCW; motor_state[2].level = 0; //set_motor_power(MOTOR_CH0,-10); //SCI_transmit_string('Hello!\n'); SCI_write('1'); SCI_write('2'); SCI_write('3'); SCI_write('4'); while(1){ pulse_buffer[PRP_STICK_R_X] = prp_get_pulse_width(PRP_STICK_R_X); pulse_buffer[PRP_STICK_R_Y] = prp_get_pulse_width(PRP_STICK_R_Y); pulse_buffer[PRP_STICK_L_X] = prp_get_pulse_width(PRP_STICK_L_X); pulse_buffer[PRP_STICK_L_Y] = prp_get_pulse_width(PRP_STICK_L_Y); pulse_buffer[PRP_SWITCH] = prp_get_pulse_width(PRP_SWITCH); pulse_buffer[PRP_VOLUME] = prp_get_pulse_width(PRP_VOLUME); system_enable = pulse_buffer[PRP_SWITCH];/* sprintf(tx_buffer,"ch0 %d ch1 %d ch2 %d ch3 %d ch4 %d ch5 %d\n", pulse_buffer[0],pulse_buffer[1],pulse_buffer[2], pulse_buffer[3],pulse_buffer[4],pulse_buffer[5]); SCI_transmit_string(tx_buffer);*/ //sprintf(tx_buffer,"sw = %d vol = %d",propo_pulse[PRP_VOLUME].def_time>>5,propo_pulse[PRP_SWITCH].def_time>>5); //SCI_transmit_string(tx_buffer); wait_msec(30); //直行座標を極座標に変換する //上手くいってない theta = atan2(pulse_buffer[PRP_STICK_L_Y],pulse_buffer[PRP_STICK_L_X]); r = sqrt((uint32_t)pulse_buffer[PRP_STICK_L_Y] * (uint32_t)pulse_buffer[PRP_STICK_L_Y] + (uint32_t)pulse_buffer[PRP_STICK_L_X] * (uint32_t)pulse_buffer[PRP_STICK_L_X]); angle_int = theta * 100; //sprintf(tx_buffer,"r = %d theta =%d\n",r,angle_int); SCI_transmit_string(tx_buffer); //sprintf(tx_buffer,"r = %lf theta =%lf\n",r,theta); power_r = 1.5*r * cos(theta - 3.141592 / 4); power_l = 1.5*r * sin(theta - 3.141592 / 4); sprintf(tx_buffer,"power_r = %d power_l = %d",power_r,power_l); SCI_transmit_string(tx_buffer); set_motor_power(MOTOR_CH0,power_r); set_motor_power(MOTOR_CH1,power_l); set_motor_power(MOTOR_CH2,-pulse_buffer[PRP_STICK_R_Y]);/* sprintf(tx_buffer,"ch3 %d\n",propo_pulse[3].def_time); SCI_transmit_string(tx_buffer); wait_msec(30);*/ //SCI_write('a'); //直行座標を極座標に変換する //上手くいってない/* theta = atan2(pulse_buffer[PRP_STICK_L_Y],pulse_buffer[PRP_STICK_L_X]); r = sqrt(pulse_buffer[PRP_STICK_L_Y] * pulse_buffer[PRP_STICK_L_Y] + pulse_buffer[PRP_STICK_L_X] * pulse_buffer[PRP_STICK_L_X]); power_r = r * cos(theta - 3.141592 / 4); power_l = r * sin(theta - 3.141592 / 4);*//* sprintf(tx_buffer,"ch0 %d rpm\n",get_motor_speed(MOTOR_CH0)); SCI_transmit_string(tx_buffer); wait_msec(5); motor_speed_control(3000);*/ } for(;;); return 0;}//モータのPWMに値をセットする//レベルの範囲は-255 ~ 255 まで//レベルが正で正転 負で逆回転//レベル範囲がおかしいときは上限、加減でリミットさせるvoid set_motor_power(uint8_t channel,int16_t level){ int16_t abs; if(level < -255){ //上下限リミット level = -255; }else if(level > 255){ level = 255; } if(level < 0){ //絶対値を得る abs = -level; }else{ abs = level; } switch (channel){ case MOTOR_CH0: motor_state[MOTOR_CH0].level = abs; if(level > 0){ motor_state[MOTOR_CH0].direction = MOTOR_CH0_CW; }else{ motor_state[MOTOR_CH0].direction = MOTOR_CH0_CCW; } break; case MOTOR_CH1: motor_state[MOTOR_CH1].level = abs; if(level > 0){ motor_state[MOTOR_CH1].direction = MOTOR_CH1_CW; }else{ motor_state[MOTOR_CH1].direction = MOTOR_CH1_CCW; } break; case MOTOR_CH2: motor_state[MOTOR_CH2].level = abs; if(level > 0){ motor_state[MOTOR_CH2].direction = MOTOR_CH2_CW; }else{ motor_state[MOTOR_CH2].direction = MOTOR_CH2_CCW; } break; }}#define PRP_PULSE_WIDTH_UPPER_LIMIT 300#define PRP_PULSE_WIDTH_LOWER_LIMIT -300#define PRP_PULSE_WIDTH_NO_EFFECT_RANGE 20#define PRP_PULSE_WIDTH_CENTER 950#define PRP_PULSE_WIDTH_SWITCH_THRESHOLD 1000#define PRP_PULSE_WIDTH_VOLUME_LOWER 610//プロポのパルス幅を-255から255に丸めた値を返す//パルス幅が異常なときは0を返す//不感帯の処理も施すint16_t prp_get_pulse_width(uint8_t channel){ int16_t buf; int16_t abs; buf = propo_pulse[channel].def_time>>5; //return buf; if(channel == PRP_SWITCH){ if(buf > PRP_PULSE_WIDTH_SWITCH_THRESHOLD){ return 1; }else{ return 0; } }else if(channel == PRP_VOLUME){ buf -= PRP_PULSE_WIDTH_VOLUME_LOWER; if(buf < 0){ buf = 0; } return buf>>2; } buf -= PRP_PULSE_WIDTH_CENTER; //原点へ移動/* switch(channel){ case PRP_STICK_R_Y: case PRP_STICK_L_Y: buf = -buf; } */ if((buf > PRP_PULSE_WIDTH_UPPER_LIMIT) || (buf < PRP_PULSE_WIDTH_LOWER_LIMIT) ){ return 0; } if(buf < 0){ abs = -buf; }else{ abs = buf; } if(abs < PRP_PULSE_WIDTH_NO_EFFECT_RANGE){ return 0; } if(buf > 0){ if(buf > 255){ buf = 255; } return buf; }else{ if(buf < -255){ buf = -255; } return buf; }}/*int16_t prp_get_pulse_width(uint8_t channel){ int16_t buf; int16_t abs; buf = propo_pulse[channel].def_time>>5; //return buf; buf -= PRP_PULSE_WIDTH_CENTER; if((buf > PRP_PULSE_WIDTH_UPPER_LIMIT) || (buf < PRP_PULSE_WIDTH_LOWER_LIMIT) ){ return 0; } if(buf < 0){ abs = -buf; }else{ abs = buf; } if(abs < PRP_PULSE_WIDTH_NO_EFFECT_RANGE){ return 0; } if(buf > 0){ if(buf > 255){ buf = 255; } return buf; }else{ if(buf < -255){ buf = -255; } return buf; }}*///タイマ1コンペア割り込み//割り込み周波数400HzISR (TIMER1_COMPA_vect){ static uint8_t divider_second; time_t *p; p = &time; p->accumulator_byte++; if(++divider_second >= 4){ divider_second = 0; //100Hzで実行される行 p->m_second += 10; p->accumulator += 10; if(p->m_second >= 1000){ p->m_second = 0; //1Hzで実行される行 if(++p->second >= 60){ p->second = 0; if(++p->minute >= 60){ p->minute = 0; if(++p->hour >= 24){ p->hour = 0; } } } } }}//ロータリーエンコーダch0,1の割り込み//立ちあがりでシステム時間をストアするISR (PCINT2_vect){ static uint8_t port_state_old; static uint8_t accumulator_byte_old; static uint16_t counter_old; uint8_t accumulator_byte_store; uint16_t counter_store; uint8_t port_state_now; uint8_t pulse_state; uint8_t accumulator_def; uint16_t counter_def; port_state_now = PIND; pulse_state = (~port_state_old) & port_state_now; if((pulse_state & 0x40) != 0){ //ch0立ち上がり検出 //SCI_write('@'); accumulator_byte_store = time.accumulator_byte; counter_store = TCNT1; //現在時刻保存 //コンペアフラグが立ってたら if(bit_is_set(TIFR1,OCF1A)){ accumulator_byte_store++; } //前回との差をとる //def = store - old //下位の処理 if(counter_store > counter_old){ counter_def = counter_store - counter_old; }else{ counter_def = RTC_COMPARE - counter_old + counter_store; accumulator_byte_old++; //桁下がりがあったから上位から引く } //上位の処理 accumulator_def = accumulator_byte_store - accumulator_byte_old; //格納 encoder_state[MOTOR_CH0].accumulator_byte_def = accumulator_def; encoder_state[MOTOR_CH0].counter_def = counter_def; accumulator_byte_old = accumulator_byte_store; //時刻保存 counter_old = counter_store; } port_state_old = port_state_now; //状態保存}//パルスデコード//パルス立ち上がりエッジのシステム時間と立下りエッジのシステム時間をそれぞれストアするISR (PCINT1_vect){ static uint8_t port_state_old; uint8_t port_state_now; uint8_t pulse_def; uint16_t counter_store; port_state_now = PINC; counter_store = TCNT1; //SCI_write('P'); pulse_def = (~port_state_old) & port_state_now; if(pulse_def & 0x01){ //アタックのビットが立ったところに現在時刻をストア propo_pulse[0].old_time = counter_store; } if(pulse_def & 0x02){ propo_pulse[1].old_time = counter_store; } if(pulse_def & 0x04){ propo_pulse[2].old_time = counter_store; } if(pulse_def & 0x08){ propo_pulse[3].old_time = counter_store; } if(pulse_def & 0x10){ propo_pulse[4].old_time = counter_store; } if(pulse_def & 0x20){ propo_pulse[5].old_time = counter_store; } pulse_def = (~port_state_now) & port_state_old; port_state_old = port_state_now; //データ更新 //リリースのビットが立ったところで時間の差をとって格納 if(pulse_def & 0x01){ if(counter_store > propo_pulse[0].old_time){ propo_pulse[0].def_time = counter_store - propo_pulse[0].old_time; }else{ propo_pulse[0].def_time = RTC_COMPARE - propo_pulse[0].old_time + counter_store; } } if(pulse_def & 0x02){ if(counter_store > propo_pulse[1].old_time){ propo_pulse[1].def_time = counter_store - propo_pulse[1].old_time; }else{ propo_pulse[1].def_time = RTC_COMPARE - propo_pulse[1].old_time + counter_store; } } if(pulse_def & 0x04){ if(counter_store > propo_pulse[2].old_time){ propo_pulse[2].def_time = counter_store - propo_pulse[2].old_time; }else{ propo_pulse[2].def_time = RTC_COMPARE - propo_pulse[2].old_time + counter_store; } } if(pulse_def & 0x08){ if(counter_store > propo_pulse[3].old_time){ propo_pulse[3].def_time = counter_store - propo_pulse[3].old_time; }else{ propo_pulse[3].def_time = RTC_COMPARE - propo_pulse[3].old_time + counter_store; } } if(pulse_def & 0x10){ if(counter_store > propo_pulse[4].old_time){ propo_pulse[4].def_time = counter_store - propo_pulse[4].old_time; }else{ propo_pulse[4].def_time = RTC_COMPARE - propo_pulse[4].old_time + counter_store; } } if(pulse_def & 0x20){ if(counter_store > propo_pulse[5].old_time){ propo_pulse[5].def_time = counter_store - propo_pulse[5].old_time; }else{ propo_pulse[5].def_time = RTC_COMPARE - propo_pulse[5].old_time + counter_store; } }}void wait_msec(uint16_t term){ uint32_t store; volatile uint8_t dummy; store = time.accumulator + (uint32_t)term; while(time.accumulator < store){ dummy++; }}void SCI_open(uint16_t bps_value){ UBRR0 = bps_value; //ボーレートセット sbi(UCSR0B,TXEN0); //送信許可 sbi(UCSR0B,RXEN0); }void SCI_write(uint8_t tx){ while (!(UCSR0A & (1<<UDRE0)) ); if(tx == 0){ tx = '_'; //NULL置き換え } UDR0 = tx;}uint8_t SCI_read(void){ //while ( !(UCSR0A & (1<<RXC0)) ); return UDR0;}void SCI_transmit_string(char *string){ while(*string){ SCI_write(*string++); } SCI_write('\n'); SCI_write('\r');}//数値をアスキー文字列に変換//ゼロサプレス有り//ゼロはスペースに変換されるchar *sitoa(uint16_t data,char *s){ char zf,c; char *head = s; zf=0; c='0'; while(data>=10000){ data -= 10000; c++; zf=1; } if (zf==0){ c=' '; } *s++=c; c='0'; while (data>=1000){ data -= 1000; c++; zf=1; } if(zf==0){ c=' '; } *s++=c; c='0'; while(data>=100){ data -= 100; c++; zf=1; } if(zf==0){ c=' '; } *s++=c; c='0'; while((uint8_t)data>=10){ data -= 10; c++; zf=1; } if(zf==0){ c=' '; } *s++=c; *s++='0'+(uint8_t)data; *s=0; //終端付加 return head;}
Unfortunatelly there is a problem: the circuit was heavy. The total payload (3motors+sonar+controller+circuit+camera+battery) must weight less then 150g.
consider getting your circuit made as a PCB using SMT parts
void radio(void){//ラジコンの出力パルスの幅を計る関数 int i; //DDRA &= ~(1<<DDA0); //PA0<->chan1 for(i=0;i<2;i++){ while((PINA&(1<<PINA0))==0){} TCNT0=0; TCCR0|=(1<<CS02); while((PINA&(1<<PINA0))==1){} TCCR0&=~(1<<CS02); if(i==1){ chanVal[CHAN1]=TCNT0; //printf("\r chan1 = %d\n",chanVal[CHAN1]); } } wait_ms(2); //DDRA &= ~(1<<DDA1); //PA1<->chan2 for(i=0;i<2;i++){ while((PINA&(1<<PINA1))==0){} TCNT0=0; TCCR0|=(1<<CS02); while((PINA&(1<<PINA1))!=0){} TCCR0&=~(1<<CS02); if(i==1){ chanVal[CHAN2]=TCNT0; //printf("\r chan2 = %d\n",chanVal[CHAN2]); } } wait_ms(2); //DDRA &= ~(1<<DDA2); //PA2<->chan3 for(i=0;i<2;i++){ while((PINA&(1<<PINA2))==0){} TCNT0=0; TCCR0|=(1<<CS02); while((PINA&(1<<PINA2))!=0){} TCCR0&=~(1<<CS02); if(i==1){ chanVal[CHAN3]=TCNT0; //printf("\r chan3 = %d\n",chanVal[CHAN3]); } } wait_ms(2); //DDRA &= ~(1<<DDA3); //PA3<->chan4 for(i=0;i<2;i++){ while((PINA&(1<<PINA3))==0){} TCNT0=0; TCCR0|=(1<<CS02); while((PINA&(1<<PINA3))!=0){} TCCR0&=~(1<<CS02); if(i==1){ chanVal[CHAN4]=TCNT0; //printf("\r chan4 = %d\n",chanVal[CHAN4]); } } wait_ms(2); //DDRA &= ~(1<<DDA4); //PA4<->chan5 for(i=0;i<2;i++){ while((PINA&(1<<PINA4))==0){} TCNT0=0; TCCR0|=(1<<CS02); while((PINA&(1<<PINA4))!=0){} TCCR0&=~(1<<CS02); if(i==1){ chanVal[CHAN5]=TCNT0; //printf("\r chan5 = %d\n",chanVal[CHAN5]); } } wait_ms(2); //DDRA &= ~(1<<DDA5); //PA5<->chan6 for(i=0;i<2;i++){ while((PINA&(1<<PINA5))==0){} TCNT0=0; TCCR0|=(1<<CS02); while((PINA&(1<<PINA5))!=0){} TCCR0&=~(1<<CS02); if(i==1){ chanVal[CHAN6]=TCNT0; //printf("\r chan6 = %d\n",chanVal[CHAN6]); } } wait_ms(2);}