ROBOSTEP_3rd_SHARE / Mbed 2 deprecated PS3conOut2

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 ROBOSTEP_3rd_SHARE

User.cpp@1:7b9e3032bb7b, 2015-05-18 (annotated)

Committer:
ideguti
Date:
Mon May 18 03:50:39 2015 +0000
Revision:
1:7b9e3032bb7b
Parent:
0:0805c5a1b328
Child:
3:4d19e16fdf4c
ps3con kiban program

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 //Ticker ticker1;
ideguti 1:7b9e3032bb7b 27 //Timer timer1;
ideguti 1:7b9e3032bb7b 28 SPI slave(p5, p6, p7);
ideguti 0:0805c5a1b328 29 CAN can1(p9, p10);
ideguti 0:0805c5a1b328 30 CANMessage msg;
ideguti 0:0805c5a1b328 31 // pin settings
ideguti 1:7b9e3032bb7b 32 //DigitalOut air1(p5);
ideguti 1:7b9e3032bb7b 33 //DigitalOut air2(p6);
ideguti 0:0805c5a1b328 34 //SPI settings//
ideguti 1:7b9e3032bb7b 35
ideguti 1:7b9e3032bb7b 36 //SPI spi(p11,p12,p13);
ideguti 1:7b9e3032bb7b 37 //DigitalOut ss1(p20);
ideguti 1:7b9e3032bb7b 38 //DigitalOut ss2(p19);
ideguti 1:7b9e3032bb7b 39 //DigitalOut ss3(p18);
ideguti 1:7b9e3032bb7b 40 //DigitalOut ss4(p17);
ideguti 0:0805c5a1b328 41 //PINsettings
ideguti 1:7b9e3032bb7b 42 //DigitalOut ldac1(p21);
ideguti 1:7b9e3032bb7b 43 //DigitalOut ldac2(p22);
ideguti 1:7b9e3032bb7b 44 //DigitalOut ldac3(p23);
ideguti 1:7b9e3032bb7b 45 //DigitalOut ldac4(p24);
ideguti 0:0805c5a1b328 46 // LED settings//
ideguti 0:0805c5a1b328 47 DigitalOut led1(LED1);
ideguti 0:0805c5a1b328 48 DigitalOut led2(LED2);
ideguti 0:0805c5a1b328 49 DigitalOut led3(LED3);
ideguti 0:0805c5a1b328 50 DigitalOut led4(LED4);
ideguti 0:0805c5a1b328 51 //
ideguti 0:0805c5a1b328 52 int serial_counter = 0;
ideguti 0:0805c5a1b328 53 int dac_data=0;
ideguti 0:0805c5a1b328 54 double debug_val = 0, debug_val2 = 0;
ideguti 0:0805c5a1b328 55 double real_val[4] = {};
ideguti 0:0805c5a1b328 56 double tar_val[4] = {};
ideguti 0:0805c5a1b328 57 double val[4] = {};
ideguti 0:0805c5a1b328 58 int send_val[4] = {};
ideguti 0:0805c5a1b328 59 double c_accel=5000;
ideguti 0:0805c5a1b328 60 char teisu=13;//23;
ideguti 0:0805c5a1b328 61 char can_send_data[8];
ideguti 0:0805c5a1b328 62
ideguti 0:0805c5a1b328 63 int con_x;
ideguti 0:0805c5a1b328 64 int con_y;
ideguti 0:0805c5a1b328 65 int con_theta;
ideguti 0:0805c5a1b328 66 double val_ratio, math1;
ideguti 0:0805c5a1b328 67 double sai_time[5]={}, sai_hensa[5]={};
ideguti 0:0805c5a1b328 68
ideguti 0:0805c5a1b328 69 long angle_ulong;
ideguti 0:0805c5a1b328 70 float raw_angle;
ideguti 0:0805c5a1b328 71 float angle;
ideguti 0:0805c5a1b328 72 float axis_angle = 0;
ideguti 0:0805c5a1b328 73 double sin2, cos2;
ideguti 0:0805c5a1b328 74 float kitai_angle = 0;
ideguti 0:0805c5a1b328 75 bool caninit_triger = 0;
ideguti 0:0805c5a1b328 76 int can_counter = 0;
ideguti 0:0805c5a1b328 77 double avr_accel_x, avr_accel_y;
ideguti 0:0805c5a1b328 78
ideguti 0:0805c5a1b328 79
ideguti 0:0805c5a1b328 80 //
ideguti 0:0805c5a1b328 81 u8 RSX,RSY,LSX,LSY,BSU,BSL;
ideguti 0:0805c5a1b328 82 //コントローラ断線時対策用
ideguti 0:0805c5a1b328 83 u8 accel[5];
ideguti 0:0805c5a1b328 84 u8 accel_nowsub=0;
ideguti 0:0805c5a1b328 85 //////////
ideguti 0:0805c5a1b328 86 char cnt=0;
ideguti 0:0805c5a1b328 87
ideguti 0:0805c5a1b328 88
ideguti 0:0805c5a1b328 89 void UserLoopSetting(){
ideguti 1:7b9e3032bb7b 90 //spi.format(16,0);
ideguti 1:7b9e3032bb7b 91 //spi.frequency(1000000);//1M
ideguti 1:7b9e3032bb7b 92 /*
ideguti 0:0805c5a1b328 93 ss1=1;
ideguti 0:0805c5a1b328 94 ss2=1;
ideguti 0:0805c5a1b328 95 ss3=1;
ideguti 0:0805c5a1b328 96 ss4=1;
ideguti 0:0805c5a1b328 97 ldac1=1;
ideguti 0:0805c5a1b328 98 ldac2=1;
ideguti 0:0805c5a1b328 99 ldac3=1;
ideguti 0:0805c5a1b328 100 ldac4=1;
ideguti 1:7b9e3032bb7b 101 */
ideguti 1:7b9e3032bb7b 102 can1.frequency(125000);
ideguti 1:7b9e3032bb7b 103
ideguti 1:7b9e3032bb7b 104 slave.format(16,0);
ideguti 1:7b9e3032bb7b 105 slave.frequency(1000000);
ideguti 1:7b9e3032bb7b 106
ideguti 0:0805c5a1b328 107 }
ideguti 0:0805c5a1b328 108
ideguti 0:0805c5a1b328 109 void UserLoop(char n,const u8* data){
ideguti 0:0805c5a1b328 110 //PS3conSTATE--->value
ideguti 0:0805c5a1b328 111 u16 ButtonState;
ideguti 1:7b9e3032bb7b 112 char Send_data[8] = {};
ideguti 1:7b9e3032bb7b 113 u16 spi_send_data[10] = {};
ideguti 0:0805c5a1b328 114 if(n==0){//有線Ps3USB.cpp
ideguti 0:0805c5a1b328 115 RSX = ((ps3report*)data)->RightStickX;
ideguti 0:0805c5a1b328 116 RSY = ((ps3report*)data)->RightStickY;
ideguti 0:0805c5a1b328 117 LSX = ((ps3report*)data)->LeftStickX;
ideguti 0:0805c5a1b328 118 LSY = ((ps3report*)data)->LeftStickY;
ideguti 0:0805c5a1b328 119 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 120 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
ideguti 0:0805c5a1b328 121 //ボタンの処理
ideguti 0:0805c5a1b328 122 ButtonState = ((ps3report*)data)->ButtonState;
ideguti 0:0805c5a1b328 123 }else {//無線TestShell.cpp
ideguti 0:0805c5a1b328 124 RSX = ((ps3report*)(data + 1))->RightStickX;
ideguti 0:0805c5a1b328 125 RSY = ((ps3report*)(data + 1))->RightStickY;
ideguti 0:0805c5a1b328 126 LSX = ((ps3report*)(data + 1))->LeftStickX;
ideguti 0:0805c5a1b328 127 LSY = ((ps3report*)(data + 1))->LeftStickY;
ideguti 0:0805c5a1b328 128 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 129 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
ideguti 0:0805c5a1b328 130 //ボタンの処理
ideguti 0:0805c5a1b328 131 ButtonState = ((ps3report*)(data + 1))->ButtonState;
ideguti 0:0805c5a1b328 132 }
ideguti 1:7b9e3032bb7b 133 Send_data[0] = RSX;
ideguti 1:7b9e3032bb7b 134 Send_data[1] = RSY;
ideguti 1:7b9e3032bb7b 135 Send_data[2] = LSX;
ideguti 1:7b9e3032bb7b 136 Send_data[3] = LSY;
ideguti 1:7b9e3032bb7b 137 Send_data[4] = BSU;
ideguti 1:7b9e3032bb7b 138 Send_data[5] = BSL;
ideguti 1:7b9e3032bb7b 139 Send_data[6] = ButtonState & 0xFF;
ideguti 1:7b9e3032bb7b 140 Send_data[7] = (ButtonState >> 8) & 0xFF;
ideguti 1:7b9e3032bb7b 141 can1.write(CANMessage(100, &Send_data[0], 8));
ideguti 1:7b9e3032bb7b 142
ideguti 1:7b9e3032bb7b 143 static int can1_counter = 0;
ideguti 1:7b9e3032bb7b 144 can1_counter ++;
ideguti 1:7b9e3032bb7b 145 if(can1_counter > 100){
ideguti 1:7b9e3032bb7b 146 led1 = !led1;
ideguti 1:7b9e3032bb7b 147 can1_counter = 0;
ideguti 1:7b9e3032bb7b 148 }
ideguti 1:7b9e3032bb7b 149 /*
ideguti 1:7b9e3032bb7b 150 spi_send_data[0] = ButtonState;
ideguti 1:7b9e3032bb7b 151 spi_send_data[1] = RSX;
ideguti 1:7b9e3032bb7b 152 spi_send_data[1] = (spi_send_data[1] << 8) & 0xFF00;
ideguti 1:7b9e3032bb7b 153 spi_send_data[1] = spi_send_data[1] | RSY;
ideguti 1:7b9e3032bb7b 154 spi_send_data[2] = RSX;
ideguti 1:7b9e3032bb7b 155 spi_send_data[2] = (spi_send_data[2] << 8) & 0xFF00;
ideguti 1:7b9e3032bb7b 156 spi_send_data[2] = spi_send_data[2] | RSY;
ideguti 1:7b9e3032bb7b 157 */
ideguti 1:7b9e3032bb7b 158 /*
ideguti 1:7b9e3032bb7b 159 if(slave.receive()){
ideguti 1:7b9e3032bb7b 160 int recive = slave.read();
ideguti 1:7b9e3032bb7b 161 switch(recive){
ideguti 1:7b9e3032bb7b 162 case 1:
ideguti 1:7b9e3032bb7b 163 slave.reply(spi_send_data[0]);
ideguti 1:7b9e3032bb7b 164 break;
ideguti 1:7b9e3032bb7b 165 case 2:
ideguti 1:7b9e3032bb7b 166 slave.reply(spi_send_data[1]);
ideguti 1:7b9e3032bb7b 167 break;
ideguti 1:7b9e3032bb7b 168 case 3:
ideguti 1:7b9e3032bb7b 169 slave.reply(spi_send_data[2]);
ideguti 1:7b9e3032bb7b 170 break;
ideguti 1:7b9e3032bb7b 171 }
ideguti 1:7b9e3032bb7b 172 }
ideguti 1:7b9e3032bb7b 173 */
ideguti 0:0805c5a1b328 174
ideguti 0:0805c5a1b328 175
ideguti 1:7b9e3032bb7b 176
ideguti 1:7b9e3032bb7b 177 //slave.write(1);
ideguti 1:7b9e3032bb7b 178
ideguti 0:0805c5a1b328 179
ideguti 0:0805c5a1b328 180
ideguti 1:7b9e3032bb7b 181
ideguti 0:0805c5a1b328 182
ideguti 0:0805c5a1b328 183
ideguti 0:0805c5a1b328 184
ideguti 0:0805c5a1b328 185
ideguti 0:0805c5a1b328 186
ideguti 1:7b9e3032bb7b 187 //can1.read(msg); //CAN読み取り
ideguti 0:0805c5a1b328 188
ideguti 0:0805c5a1b328 189
ideguti 0:0805c5a1b328 190
ideguti 0:0805c5a1b328 191
ideguti 0:0805c5a1b328 192
ideguti 0:0805c5a1b328 193
ideguti 0:0805c5a1b328 194
ideguti 0:0805c5a1b328 195
ideguti 0:0805c5a1b328 196
ideguti 0:0805c5a1b328 197
ideguti 0:0805c5a1b328 198
ideguti 0:0805c5a1b328 199 //LRの処理
ideguti 0:0805c5a1b328 200
ideguti 0:0805c5a1b328 201
ideguti 0:0805c5a1b328 202
ideguti 0:0805c5a1b328 203 serial_counter ++;
ideguti 0:0805c5a1b328 204
ideguti 0:0805c5a1b328 205
ideguti 0:0805c5a1b328 206 #ifdef DEBUG_MODE
ideguti 0:0805c5a1b328 207 if(serial_counter > REFRESHRATE_PRINTF){
ideguti 0:0805c5a1b328 208 printf("angle %f",angle*180/M_PI);
ideguti 0:0805c5a1b328 209 printf("\t avr_accel_x %f",avr_accel_x);
ideguti 0:0805c5a1b328 210 printf("\t math1 %f",math1);
ideguti 0:0805c5a1b328 211 printf("\t c_accel %f",c_accel);
ideguti 0:0805c5a1b328 212 //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
ideguti 0:0805c5a1b328 213 printf("\t kitai_angle %f",kitai_angle*180/M_PI);
ideguti 0:0805c5a1b328 214 printf("\n");
ideguti 0:0805c5a1b328 215
ideguti 0:0805c5a1b328 216
ideguti 0:0805c5a1b328 217 printf("data[0] %f",val[0]);
ideguti 0:0805c5a1b328 218 printf("\t");
ideguti 0:0805c5a1b328 219 printf("data[1] %f",val[1]);
ideguti 0:0805c5a1b328 220 printf("\t");
ideguti 0:0805c5a1b328 221 printf("data[2] %f",val[2]);
ideguti 0:0805c5a1b328 222 printf("\t");
ideguti 0:0805c5a1b328 223 printf("data[3] %f",val[3]);
ideguti 0:0805c5a1b328 224 printf("\n");
ideguti 0:0805c5a1b328 225
ideguti 0:0805c5a1b328 226 serial_counter = 0;
ideguti 0:0805c5a1b328 227 }
ideguti 0:0805c5a1b328 228 #endif
ideguti 0:0805c5a1b328 229
ideguti 0:0805c5a1b328 230
ideguti 0:0805c5a1b328 231
ideguti 0:0805c5a1b328 232
ideguti 0:0805c5a1b328 233
ideguti 0:0805c5a1b328 234 #endif
ideguti 0:0805c5a1b328 235
ideguti 0:0805c5a1b328 236
ideguti 0:0805c5a1b328 237
ideguti 0:0805c5a1b328 238 }