//ソースコード: 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;//データが受信できているかを監視
}
}
}