Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of PS3conOut by
User.cpp@3:4d19e16fdf4c, 2015-10-14 (annotated)
- 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?
User | Revision | Line number | New 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 | } |