ROBOSTEP_3rd_SHARE / Mbed 2 deprecated PS3conOut2

Dependencies:   mbed

Fork of PS3conOut by ROBOSTEP_3rd_SHARE

Committer:
ShotaroSenzaki
Date:
Wed Oct 14 12:24:17 2015 +0000
Revision:
3:4d19e16fdf4c
Parent:
1:7b9e3032bb7b
Child:
4:ee01e28db9d4
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ideguti 0:0805c5a1b328 1
ideguti 0:0805c5a1b328 2
ideguti 0:0805c5a1b328 3
ideguti 0:0805c5a1b328 4 #include <math.h>
ideguti 0:0805c5a1b328 5 #include "Utils.h"
ideguti 0:0805c5a1b328 6 #include "USBHost.h"
ideguti 0:0805c5a1b328 7 #include "hci.h"
ideguti 0:0805c5a1b328 8 #include "ps3.h"
ideguti 0:0805c5a1b328 9 #include "TestShell.h"
ideguti 0:0805c5a1b328 10 #include "User.h"
ideguti 0:0805c5a1b328 11 #include "caninit.h"
ideguti 0:0805c5a1b328 12 #include "mbed.h"
ideguti 0:0805c5a1b328 13 #ifndef M_PI
ideguti 0:0805c5a1b328 14 #define M_PI 3.14159265f
ideguti 0:0805c5a1b328 15 #endif
ideguti 0:0805c5a1b328 16 #ifdef common
ideguti 0:0805c5a1b328 17 #define ab 15
ideguti 0:0805c5a1b328 18 #define buf 14
ideguti 0:0805c5a1b328 19 #define ga 13
ideguti 0:0805c5a1b328 20 #define shdn 12
ideguti 0:0805c5a1b328 21 #define LSXcent 128
ideguti 0:0805c5a1b328 22 #define LSYcent 128
ideguti 0:0805c5a1b328 23 #define RSXcent 128
ideguti 0:0805c5a1b328 24 #define RSYcent 128
ideguti 0:0805c5a1b328 25
ideguti 0:0805c5a1b328 26 CAN can1(p9, p10);
ideguti 0:0805c5a1b328 27 CANMessage msg;
ideguti 0:0805c5a1b328 28 // pin settings
ideguti 0:0805c5a1b328 29 DigitalOut led1(LED1);
ideguti 0:0805c5a1b328 30 DigitalOut led2(LED2);
ideguti 0:0805c5a1b328 31 DigitalOut led3(LED3);
ideguti 0:0805c5a1b328 32 DigitalOut led4(LED4);
ShotaroSenzaki 3:4d19e16fdf4c 33
ideguti 0:0805c5a1b328 34 int serial_counter = 0;
ideguti 0:0805c5a1b328 35 int dac_data=0;
ideguti 0:0805c5a1b328 36 double debug_val = 0, debug_val2 = 0;
ideguti 0:0805c5a1b328 37 double real_val[4] = {};
ideguti 0:0805c5a1b328 38 double tar_val[4] = {};
ideguti 0:0805c5a1b328 39 double val[4] = {};
ideguti 0:0805c5a1b328 40 int send_val[4] = {};
ideguti 0:0805c5a1b328 41 double c_accel=5000;
ideguti 0:0805c5a1b328 42 char teisu=13;//23;
ideguti 0:0805c5a1b328 43 char can_send_data[8];
ideguti 0:0805c5a1b328 44
ideguti 0:0805c5a1b328 45 int con_x;
ideguti 0:0805c5a1b328 46 int con_y;
ideguti 0:0805c5a1b328 47 int con_theta;
ideguti 0:0805c5a1b328 48 double val_ratio, math1;
ideguti 0:0805c5a1b328 49 double sai_time[5]={}, sai_hensa[5]={};
ideguti 0:0805c5a1b328 50
ideguti 0:0805c5a1b328 51 long angle_ulong;
ideguti 0:0805c5a1b328 52 float raw_angle;
ideguti 0:0805c5a1b328 53 float angle;
ideguti 0:0805c5a1b328 54 float axis_angle = 0;
ideguti 0:0805c5a1b328 55 double sin2, cos2;
ideguti 0:0805c5a1b328 56 float kitai_angle = 0;
ideguti 0:0805c5a1b328 57 bool caninit_triger = 0;
ideguti 0:0805c5a1b328 58 int can_counter = 0;
ideguti 0:0805c5a1b328 59 double avr_accel_x, avr_accel_y;
ideguti 0:0805c5a1b328 60
ideguti 0:0805c5a1b328 61
ideguti 0:0805c5a1b328 62 //
ideguti 0:0805c5a1b328 63 u8 RSX,RSY,LSX,LSY,BSU,BSL;
ideguti 0:0805c5a1b328 64 //コントローラ断線時対策用
ideguti 0:0805c5a1b328 65 u8 accel[5];
ideguti 0:0805c5a1b328 66 u8 accel_nowsub=0;
ideguti 0:0805c5a1b328 67 //////////
ideguti 0:0805c5a1b328 68 char cnt=0;
ideguti 0:0805c5a1b328 69
ideguti 0:0805c5a1b328 70
ideguti 0:0805c5a1b328 71 void UserLoopSetting(){
ShotaroSenzaki 3:4d19e16fdf4c 72 can1.frequency(500000);
ideguti 0:0805c5a1b328 73 }
ideguti 0:0805c5a1b328 74
ideguti 0:0805c5a1b328 75 void UserLoop(char n,const u8* data){
ideguti 0:0805c5a1b328 76 //PS3conSTATE--->value
ideguti 0:0805c5a1b328 77 u16 ButtonState;
ideguti 1:7b9e3032bb7b 78 char Send_data[8] = {};
ideguti 1:7b9e3032bb7b 79 u16 spi_send_data[10] = {};
ideguti 0:0805c5a1b328 80 if(n==0){//有線Ps3USB.cpp
ideguti 0:0805c5a1b328 81 RSX = ((ps3report*)data)->RightStickX;
ideguti 0:0805c5a1b328 82 RSY = ((ps3report*)data)->RightStickY;
ideguti 0:0805c5a1b328 83 LSX = ((ps3report*)data)->LeftStickX;
ideguti 0:0805c5a1b328 84 LSY = ((ps3report*)data)->LeftStickY;
ideguti 0:0805c5a1b328 85 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 86 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
ideguti 0:0805c5a1b328 87 //ボタンの処理
ideguti 0:0805c5a1b328 88 ButtonState = ((ps3report*)data)->ButtonState;
ideguti 0:0805c5a1b328 89 }else {//無線TestShell.cpp
ideguti 0:0805c5a1b328 90 RSX = ((ps3report*)(data + 1))->RightStickX;
ideguti 0:0805c5a1b328 91 RSY = ((ps3report*)(data + 1))->RightStickY;
ideguti 0:0805c5a1b328 92 LSX = ((ps3report*)(data + 1))->LeftStickX;
ideguti 0:0805c5a1b328 93 LSY = ((ps3report*)(data + 1))->LeftStickY;
ideguti 0:0805c5a1b328 94 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 95 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
ideguti 0:0805c5a1b328 96 //ボタンの処理
ideguti 0:0805c5a1b328 97 ButtonState = ((ps3report*)(data + 1))->ButtonState;
ideguti 0:0805c5a1b328 98 }
ShotaroSenzaki 3:4d19e16fdf4c 99 Send_data[0] = RSX; //MAX 1000 MIN -1000 MD MAX 4096 MIN -4096
ideguti 1:7b9e3032bb7b 100 Send_data[1] = RSY;
ideguti 1:7b9e3032bb7b 101 Send_data[2] = LSX;
ideguti 1:7b9e3032bb7b 102 Send_data[3] = LSY;
ideguti 1:7b9e3032bb7b 103 Send_data[4] = BSU;
ideguti 1:7b9e3032bb7b 104 Send_data[5] = BSL;
ideguti 1:7b9e3032bb7b 105 Send_data[6] = ButtonState & 0xFF;
ideguti 1:7b9e3032bb7b 106 Send_data[7] = (ButtonState >> 8) & 0xFF;
ShotaroSenzaki 3:4d19e16fdf4c 107 can1.write(CANMessage(23, &Send_data[0], 8)); //id=1 自動 id=23 手動
ideguti 1:7b9e3032bb7b 108
ideguti 1:7b9e3032bb7b 109 static int can1_counter = 0;
ideguti 1:7b9e3032bb7b 110 can1_counter ++;
ideguti 1:7b9e3032bb7b 111 if(can1_counter > 100){
ideguti 1:7b9e3032bb7b 112 led1 = !led1;
ideguti 1:7b9e3032bb7b 113 can1_counter = 0;
ideguti 1:7b9e3032bb7b 114 }
ShotaroSenzaki 3:4d19e16fdf4c 115
ideguti 0:0805c5a1b328 116 serial_counter ++;
ideguti 0:0805c5a1b328 117
ideguti 0:0805c5a1b328 118
ideguti 0:0805c5a1b328 119 #ifdef DEBUG_MODE
ideguti 0:0805c5a1b328 120 if(serial_counter > REFRESHRATE_PRINTF){
ideguti 0:0805c5a1b328 121 printf("angle %f",angle*180/M_PI);
ideguti 0:0805c5a1b328 122 printf("\t avr_accel_x %f",avr_accel_x);
ideguti 0:0805c5a1b328 123 printf("\t math1 %f",math1);
ideguti 0:0805c5a1b328 124 printf("\t c_accel %f",c_accel);
ideguti 0:0805c5a1b328 125 //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
ideguti 0:0805c5a1b328 126 printf("\t kitai_angle %f",kitai_angle*180/M_PI);
ideguti 0:0805c5a1b328 127 printf("\n");
ideguti 0:0805c5a1b328 128
ideguti 0:0805c5a1b328 129
ideguti 0:0805c5a1b328 130 printf("data[0] %f",val[0]);
ideguti 0:0805c5a1b328 131 printf("\t");
ideguti 0:0805c5a1b328 132 printf("data[1] %f",val[1]);
ideguti 0:0805c5a1b328 133 printf("\t");
ideguti 0:0805c5a1b328 134 printf("data[2] %f",val[2]);
ideguti 0:0805c5a1b328 135 printf("\t");
ideguti 0:0805c5a1b328 136 printf("data[3] %f",val[3]);
ideguti 0:0805c5a1b328 137 printf("\n");
ideguti 0:0805c5a1b328 138
ideguti 0:0805c5a1b328 139 serial_counter = 0;
ideguti 0:0805c5a1b328 140 }
ideguti 0:0805c5a1b328 141 #endif
ideguti 0:0805c5a1b328 142
ideguti 0:0805c5a1b328 143
ideguti 0:0805c5a1b328 144
ideguti 0:0805c5a1b328 145
ideguti 0:0805c5a1b328 146
ideguti 0:0805c5a1b328 147 #endif
ideguti 0:0805c5a1b328 148
ideguti 0:0805c5a1b328 149
ideguti 0:0805c5a1b328 150
ideguti 0:0805c5a1b328 151 }