「StickController Ver0.1」の編集履歴(バックアップ)一覧に戻る
- //ソースコード: polar_stick_ver4
- #include <MsTimer2.h>
- #include <math.h>
- #define MAX_X 1012
- #define MAX_Y 998
- #define MIN_X 0
- #define MIN_Y 0
- #define NEUTRAL_X 414
- #define NEUTRAL_Y 422
- #define asobi 20
- #define STICK_LED 3
- #define DATA0_PIN 5
-
- unsigned long MainTimerCount;
- /*TimerCount1: データの送信周期用
- TimerCount2: スティックボタンのチャタリング対策用
- TimerCount3: errorLed用
- TimerCount4: 受信データが途切れていないかの確認用
- error_flag: flagが1の場合はエラーなので, error_led関数がenableになる
- */
- unsigned long TimerCount1, TimerCount2, TimerCount3, TimerCount4;
- int stick_mode, error_flag, error_led_mode;
-
- /*通信が不安定になった場合に実行する*/
- void XBee_abort(int second){
- int i;
- make_command(0, 0);
- for(i = 0; i < second; i++){
- make_command(0, 0);
- delay(1000);
- Serial.flush();
- }
- }
-
- void timer_interrupt(){
- int i;
- double r, s;
- int left_power, right_power;//-255 ~ 255でモーター出力を表現
- if(MainTimerCount - TimerCount1 >= 10){//データの送信
- stick_read(&r, &s);//極座標で値を返す関数
- if(stick_mode == 0)
- convert_pwm2(r,s,&left_power, &right_power);
- else if(stick_mode == 1)
- convert_pwm(r,s,&left_power, &right_power);
- make_command(left_power, right_power);
-
- TimerCount1 = MainTimerCount;
- }
- error_led();
- //タイマーの加算
- MainTimerCount++;
- }
-
- //errorLedを点灯させる関数
- void error_led(){
- if(error_flag == 1){
- if(MainTimerCount - TimerCount3 >= 50){
- if(error_led_mode == 0){
- analogWrite(DATA0_PIN,50);
- error_led_mode = 1;
- }
- else{
- analogWrite(DATA0_PIN, 0);
- error_led_mode = 0;
- }
- TimerCount3 = MainTimerCount;
- }
- }
- }
-
- void button_interrupt(){
- if(MainTimerCount - TimerCount2 >= 50){
- if(stick_mode == 0){
- stick_mode = 1;
- analogWrite(STICK_LED, 255);
- }
- else{
- stick_mode = 0;
- analogWrite(STICK_LED, 0);
- }
- TimerCount2 = MainTimerCount;
- }
- }
-
- void setup(){
- Serial.begin(9600);
- pinMode(STICK_LED, OUTPUT);
- pinMode(DATA0_PIN, OUTPUT);
- MainTimerCount = 0;
- TimerCount1 = 0;
- TimerCount2 = 0;
- TimerCount3 = 0;
- TimerCount4 = 0;
- error_flag = 0;
- attachInterrupt(0, button_interrupt, FALLING);
- MsTimer2::set(10, timer_interrupt);
- MsTimer2::start();
- attachInterrupt(0, button_interrupt, FALLING);
- stick_mode = 0;
- digitalWrite(STICK_LED, LOW);
- analogWrite(DATA0_PIN, 0);
- }
-
- /*
- rは中央からの距離, sは角度を表す. rの最大は1である.
- sはスティックを前に倒すと0, 左に倒すと-90, 右に倒すと90, 後ろに倒すと180 or -180を返す
- */
- void stick_read(double *r, double *s){
- double stick_x, stick_y;
-
-
- //スティック位置の補正
- stick_x = (double)analogRead(0);
- stick_y = (double)analogRead(1);
-
- if(stick_x < NEUTRAL_X){
- stick_x = (stick_x - NEUTRAL_X) / (NEUTRAL_X - MIN_X);
- }
- else{
- stick_x = (stick_x - NEUTRAL_X) / (MAX_X - NEUTRAL_X);
- }
- if(stick_y < NEUTRAL_Y){
- stick_y = (stick_y -NEUTRAL_Y) / (NEUTRAL_Y - MIN_Y);
- }
- else{
- stick_y = (stick_y - NEUTRAL_Y)/ (MAX_Y - NEUTRAL_Y);
- }
- /* デバッグ用
- Serial.print(stick_x);
- Serial.print(",");
- Serial.println(stick_y);
- */
- *r = sqrt(pow(stick_x,2) + pow(stick_y, 2));
- if(*r > 1)*r = 1;
- else if(*r < -1)*r = -1;
- *s = atan2(stick_x, stick_y) /3.14 * 180;
- }
- /*
- 極座標による入力を二つのモーターの出力に変換する関数.
- 超信地展開するバージョン.
- moterの出力は-255 ~ 255である. -は逆回転を表す
- */
- void convert_pwm(double r, double s, int *lp, int *rp){
- double _lp, _rp;
- if(-180 <= s && s < -90){
- _lp = -1;
- _rp = (s + 180) / 45 - 1;
- }
- else if(-90 <= s && s < 0){
- _lp = (s + 90) / 45 - 1;
- _rp = 1;
- }
- else if(0 <= s && s < 90){
- _lp = 1;
- _rp = 1 - s / 45;
- }
- else{//90 <= r <= 180の時
- _lp = 1 - (s - 90) / 45;
- _rp = -1;
- }
- *lp = _lp * r * 255;
- *rp = _rp * r * 255;
- if(abs(*lp) < 20)*lp = 0;
- if(abs(*rp) < 20)*rp = 0;
- /*デバグ用
- Serial.print(_lp);
- Serial.print(",");
- Serial.println(_rp);
- */
- }
- /*
- 極座標による入力を二つのモーターの出力に変換する関数.
- 超信地展開しないバージョン.
- moterの出力は-255 ~ 255である. -は逆回転を表す
- */
- void convert_pwm2(double r, double s, int *lp, int *rp){
- double _lp, _rp;
- if(-180 <= s && s < -90){
- _lp = (s + 180) / 90 - 1;
- _rp = (s + 180) / 45 - 1;
- }
- else if(-90 <= s && s < 0){
- _lp = (s + 90) / 90;
- _rp = 1;
- }
- else if(0 <= s && s < 90){
- _lp = 1;
- _rp = 1 - s / 90;
- }
- else{//90 <= r <= 180の時
- _lp = 1 - (s - 90) / 45;
- _rp = -1 * (s - 90) / 90;
- }
- *lp = _lp * r * 255;
- *rp = _rp * r * 255;
- if(abs(*lp) < 20)*lp = 0;
- if(abs(*rp) < 20)*rp = 0;
- /*デバグ用
- Serial.print(_lp);
- Serial.print(",");
- Serial.println(_rp);
- */
- }
-
- void make_command(int left_power, int right_power){
- char c0, c1, c2, c3;
- left_power += 255;
- right_power += 255;
- c0 = (left_power & 0b111111100) >> 2;
- c1 = (left_power & 0b000000011) << 5;
- c1 = c1 | ((right_power & 0b111110000) >> 4);
- c2 = (right_power & 0b000001111) << 3;
- c3 = 0;
- c0 = c0 | 0b10000000;//先頭バイトの先頭ビットを1とする
- c0 = c0 & 0b111111111;
- Serial.print(c0);
- Serial.print(c1);
- Serial.print(c2);
- Serial.print(c3);
- }
-
- void loop(){
- int i, b;
- char c[12];
- int data[6];
- if(MainTimerCount - TimerCount4 >= 100){
- error_flag = 1;
- XBee_abort(3);
- error_flag = 0;
- make_command(0, 0);
- delay(50);
- TimerCount4 = MainTimerCount;
- }
- //データの読み込み
- if(Serial.available()){
- c[0] = Serial.read();
- if((c[0] & 0b10000000) != 0){
- delay(30);
- for(i = 1 ; i < 12; i++)
- c[i] =Serial.read();
- for(i = 0; i < 6; i++){
- data[i] = c[i * 2] & 0b01111111;
- data[i] = data[i] << 3;
- data[i] = data[i] | c[i * 2 + 1] & 0b00000111;
- }
- b = data[0] / 4 - 100;
- if(b < 0)b = 0;
- analogWrite(DATA0_PIN, b);
- TimerCount4 = MainTimerCount;//データが受信できているかを監視
- }
- }
- }
-