File content as of revision 5:6f6948b8056b:

#include <math.h>
#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "TestShell.h"
#include "User.h"
#include "caninit.h"
#include "mbed.h"
#ifndef M_PI
#define M_PI 3.14159265f
#endif
#ifdef common
#define ab 15
#define buf 14
#define ga 13
#define shdn 12
#define LSXcent 128
#define LSYcent 128
#define RSXcent 128
#define RSYcent 128
#define M_MAX 1000

CAN can1(p9, p10);
CANMessage msg;  
// pin settings
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

int serial_counter = 0;
int dac_data=0;
double debug_val = 0, debug_val2 = 0;
double real_val[4] = {};
double tar_val[4] = {};
double val[4] = {};
int send_val[4] = {};
double c_accel=5000;
char teisu=13;//23;
char can_send_data[8];

int con_x;
int con_y;
int con_theta;
double val_ratio, math1;
double sai_time[5]={}, sai_hensa[5]={}; 

long angle_ulong;
float raw_angle;
float angle;
float axis_angle = 0;
double sin2, cos2;
float kitai_angle = 0;
bool caninit_triger = 0;
int can_counter = 0;
double  avr_accel_x, avr_accel_y;

int motor1=0;
int motor2=0;
int motor3=0;
int motor4=0;

//
u8 RSX,RSY,LSX,LSY,BSU,BSL;
//コントローラ断線時対策用
u8 accel[5];
u8 accel_nowsub=0;
//////////
char cnt=0;


void UserLoopSetting(){
    can1.frequency(500000);
}

void UserLoop(char n,const u8* data){
                //PS3conSTATE--->value
                u16 ButtonState;
                char Send_data[8] = {};
                //u16 spi_send_data[10] = {};
                if(n==0){//有線Ps3USB.cpp
                    RSX = ((ps3report*)data)->RightStickX;
                    RSY = ((ps3report*)data)->RightStickY;
                    LSX = ((ps3report*)data)->LeftStickX;
                    LSY = ((ps3report*)data)->LeftStickY;
                    BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
                    BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
                    //ボタンの処理
                    ButtonState =  ((ps3report*)data)->ButtonState;
                }else {//無線TestShell.cpp
                    RSX = ((ps3report*)(data + 1))->RightStickX;
                    RSY = ((ps3report*)(data + 1))->RightStickY;
                    LSX = ((ps3report*)(data + 1))->LeftStickX;
                    LSY = ((ps3report*)(data + 1))->LeftStickY;
                    BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
                    BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
                    //ボタンの処理
                    ButtonState =  ((ps3report*)(data + 1))->ButtonState;
                }
                
                
                if(LSX<150 &&LSX>100 && LSY<150 && LSY>100){
                    LSX=128;
                    LSY=128;
                    }
                    /*
                else if(LSX<150 && LSX>78 && LSY>=0 && LSY<22){
                    LSX=128;
                    LSY=0;
                    }
                else if(LSX<150 && LSX>78 && LSY>233 && LSY<255){
                    LSX=128;
                    LSY=255;
                    }*/
                motor1 = (M_MAX/180)*((LSX-128)/(2^(1/2)))+((128-LSY)/(2^(1/2)));
                motor2 = (M_MAX/180)*(-(LSX-128)/(2^(1/2)))+((128-LSY)/(2^(1/2)));
                motor3 = motor1*-1;
                motor4 = motor2*-1;
                
                //Send_data[0] = RSX;             //MAX 1000 MIN -1000    MD MAX 4096 MIN -4096
                Send_data[0] = motor4 >>8;
                Send_data[1] = motor4 &&255;
                Send_data[2] = motor1 >>8;
                Send_data[3] = motor1 &&255;
                Send_data[4] = motor2 >>8;
                Send_data[5] = motor2 &&255;
                Send_data[6] = motor3 >>8;
                Send_data[7] = motor3 &&255;
                can1.write(CANMessage(1, &Send_data[0], 8));       //id=1 自動　id=23 手動
                
                static int can1_counter = 0;
                can1_counter ++;
                if(can1_counter > 100){
                    led1 = !led1;
                    can1_counter = 0;   
                }
                                            
                serial_counter ++;
                
                printf("motor1=%d,motor2=%d,motor3=%d,motor4=%d,x=%d,y=%d\n\r",motor1,motor2,motor3,motor4,LSX-128,128-LSY);
                //printf("motor1=%d,motor2=%d,motor3=%d,motor4=%d\n\r",motor1,motor2,motor3,motor4);
                //printf("x=%d,y=%d\n\r",LSX-128,128-LSY);
               
#ifdef DEBUG_MODE                
                if(serial_counter > REFRESHRATE_PRINTF){
                printf("angle %f",angle*180/M_PI);
                printf("\t avr_accel_x %f",avr_accel_x);
                printf("\t math1 %f",math1);
                printf("\t c_accel %f",c_accel);
                //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
                printf("\t kitai_angle %f",kitai_angle*180/M_PI);
                printf("\n");
                
                
                printf("data[0] %f",val[0]);
                printf("\t");
                printf("data[1] %f",val[1]);
                printf("\t");
                printf("data[2] %f",val[2]);
                printf("\t");
                printf("data[3] %f",val[3]);
                printf("\n");
                
                serial_counter = 0;
                }
#endif
                
                
                


#endif



}