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@5:6f6948b8056b, 2015-10-16 (annotated)

Committer:
yuji8822
Date:
Fri Oct 16 01:21:12 2015 +0000
Revision:
5:6f6948b8056b
Parent:
4:ee01e28db9d4 
kkkk;

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
yuji8822 5:6f6948b8056b 25 #define M_MAX 1000
ideguti 0:0805c5a1b328 26
ideguti 0:0805c5a1b328 27 CAN can1(p9, p10);
ideguti 0:0805c5a1b328 28 CANMessage msg;
ideguti 0:0805c5a1b328 29 // pin settings
ideguti 0:0805c5a1b328 30 DigitalOut led1(LED1);
ideguti 0:0805c5a1b328 31 DigitalOut led2(LED2);
ideguti 0:0805c5a1b328 32 DigitalOut led3(LED3);
ideguti 0:0805c5a1b328 33 DigitalOut led4(LED4);
ShotaroSenzaki 3:4d19e16fdf4c 34
ideguti 0:0805c5a1b328 35 int serial_counter = 0;
ideguti 0:0805c5a1b328 36 int dac_data=0;
ideguti 0:0805c5a1b328 37 double debug_val = 0, debug_val2 = 0;
ideguti 0:0805c5a1b328 38 double real_val[4] = {};
ideguti 0:0805c5a1b328 39 double tar_val[4] = {};
ideguti 0:0805c5a1b328 40 double val[4] = {};
ideguti 0:0805c5a1b328 41 int send_val[4] = {};
ideguti 0:0805c5a1b328 42 double c_accel=5000;
ideguti 0:0805c5a1b328 43 char teisu=13;//23;
ideguti 0:0805c5a1b328 44 char can_send_data[8];
ideguti 0:0805c5a1b328 45
ideguti 0:0805c5a1b328 46 int con_x;
ideguti 0:0805c5a1b328 47 int con_y;
ideguti 0:0805c5a1b328 48 int con_theta;
ideguti 0:0805c5a1b328 49 double val_ratio, math1;
ideguti 0:0805c5a1b328 50 double sai_time[5]={}, sai_hensa[5]={};
ideguti 0:0805c5a1b328 51
ideguti 0:0805c5a1b328 52 long angle_ulong;
ideguti 0:0805c5a1b328 53 float raw_angle;
ideguti 0:0805c5a1b328 54 float angle;
ideguti 0:0805c5a1b328 55 float axis_angle = 0;
ideguti 0:0805c5a1b328 56 double sin2, cos2;
ideguti 0:0805c5a1b328 57 float kitai_angle = 0;
ideguti 0:0805c5a1b328 58 bool caninit_triger = 0;
ideguti 0:0805c5a1b328 59 int can_counter = 0;
ideguti 0:0805c5a1b328 60 double avr_accel_x, avr_accel_y;
ideguti 0:0805c5a1b328 61
ShotaroSenzaki 4:ee01e28db9d4 62 int motor1=0;
ShotaroSenzaki 4:ee01e28db9d4 63 int motor2=0;
ShotaroSenzaki 4:ee01e28db9d4 64 int motor3=0;
ShotaroSenzaki 4:ee01e28db9d4 65 int motor4=0;
ideguti 0:0805c5a1b328 66
ideguti 0:0805c5a1b328 67 //
ideguti 0:0805c5a1b328 68 u8 RSX,RSY,LSX,LSY,BSU,BSL;
ideguti 0:0805c5a1b328 69 //コントローラ断線時対策用
ideguti 0:0805c5a1b328 70 u8 accel[5];
ideguti 0:0805c5a1b328 71 u8 accel_nowsub=0;
ideguti 0:0805c5a1b328 72 //////////
ideguti 0:0805c5a1b328 73 char cnt=0;
ideguti 0:0805c5a1b328 74
ideguti 0:0805c5a1b328 75
ideguti 0:0805c5a1b328 76 void UserLoopSetting(){
ShotaroSenzaki 3:4d19e16fdf4c 77 can1.frequency(500000);
ideguti 0:0805c5a1b328 78 }
ideguti 0:0805c5a1b328 79
ideguti 0:0805c5a1b328 80 void UserLoop(char n,const u8* data){
ideguti 0:0805c5a1b328 81 //PS3conSTATE--->value
ideguti 0:0805c5a1b328 82 u16 ButtonState;
ideguti 1:7b9e3032bb7b 83 char Send_data[8] = {};
ShotaroSenzaki 4:ee01e28db9d4 84 //u16 spi_send_data[10] = {};
ideguti 0:0805c5a1b328 85 if(n==0){//有線Ps3USB.cpp
ideguti 0:0805c5a1b328 86 RSX = ((ps3report*)data)->RightStickX;
ideguti 0:0805c5a1b328 87 RSY = ((ps3report*)data)->RightStickY;
ideguti 0:0805c5a1b328 88 LSX = ((ps3report*)data)->LeftStickX;
ideguti 0:0805c5a1b328 89 LSY = ((ps3report*)data)->LeftStickY;
ideguti 0:0805c5a1b328 90 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 91 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
ideguti 0:0805c5a1b328 92 //ボタンの処理
ideguti 0:0805c5a1b328 93 ButtonState = ((ps3report*)data)->ButtonState;
ideguti 0:0805c5a1b328 94 }else {//無線TestShell.cpp
ideguti 0:0805c5a1b328 95 RSX = ((ps3report*)(data + 1))->RightStickX;
ideguti 0:0805c5a1b328 96 RSY = ((ps3report*)(data + 1))->RightStickY;
ideguti 0:0805c5a1b328 97 LSX = ((ps3report*)(data + 1))->LeftStickX;
ideguti 0:0805c5a1b328 98 LSY = ((ps3report*)(data + 1))->LeftStickY;
ideguti 0:0805c5a1b328 99 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 100 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
ideguti 0:0805c5a1b328 101 //ボタンの処理
ideguti 0:0805c5a1b328 102 ButtonState = ((ps3report*)(data + 1))->ButtonState;
ideguti 0:0805c5a1b328 103 }
ShotaroSenzaki 4:ee01e28db9d4 104
yuji8822 5:6f6948b8056b 105
yuji8822 5:6f6948b8056b 106 if(LSX<150 &&LSX>100 && LSY<150 && LSY>100){
yuji8822 5:6f6948b8056b 107 LSX=128;
yuji8822 5:6f6948b8056b 108 LSY=128;
yuji8822 5:6f6948b8056b 109 }
yuji8822 5:6f6948b8056b 110 /*
yuji8822 5:6f6948b8056b 111 else if(LSX<150 && LSX>78 && LSY>=0 && LSY<22){
yuji8822 5:6f6948b8056b 112 LSX=128;
yuji8822 5:6f6948b8056b 113 LSY=0;
yuji8822 5:6f6948b8056b 114 }
yuji8822 5:6f6948b8056b 115 else if(LSX<150 && LSX>78 && LSY>233 && LSY<255){
yuji8822 5:6f6948b8056b 116 LSX=128;
yuji8822 5:6f6948b8056b 117 LSY=255;
yuji8822 5:6f6948b8056b 118 }*/
yuji8822 5:6f6948b8056b 119 motor1 = (M_MAX/180)*((LSX-128)/(2^(1/2)))+((128-LSY)/(2^(1/2)));
yuji8822 5:6f6948b8056b 120 motor2 = (M_MAX/180)*(-(LSX-128)/(2^(1/2)))+((128-LSY)/(2^(1/2)));
ShotaroSenzaki 4:ee01e28db9d4 121 motor3 = motor1*-1;
ShotaroSenzaki 4:ee01e28db9d4 122 motor4 = motor2*-1;
ShotaroSenzaki 4:ee01e28db9d4 123
ShotaroSenzaki 4:ee01e28db9d4 124 //Send_data[0] = RSX; //MAX 1000 MIN -1000 MD MAX 4096 MIN -4096
yuji8822 5:6f6948b8056b 125 Send_data[0] = motor4 >>8;
yuji8822 5:6f6948b8056b 126 Send_data[1] = motor4 &&255;
yuji8822 5:6f6948b8056b 127 Send_data[2] = motor1 >>8;
yuji8822 5:6f6948b8056b 128 Send_data[3] = motor1 &&255;
yuji8822 5:6f6948b8056b 129 Send_data[4] = motor2 >>8;
yuji8822 5:6f6948b8056b 130 Send_data[5] = motor2 &&255;
yuji8822 5:6f6948b8056b 131 Send_data[6] = motor3 >>8;
yuji8822 5:6f6948b8056b 132 Send_data[7] = motor3 &&255;
ShotaroSenzaki 4:ee01e28db9d4 133 can1.write(CANMessage(1, &Send_data[0], 8)); //id=1 自動　id=23 手動
ideguti 1:7b9e3032bb7b 134
ideguti 1:7b9e3032bb7b 135 static int can1_counter = 0;
ideguti 1:7b9e3032bb7b 136 can1_counter ++;
ideguti 1:7b9e3032bb7b 137 if(can1_counter > 100){
ideguti 1:7b9e3032bb7b 138 led1 = !led1;
ideguti 1:7b9e3032bb7b 139 can1_counter = 0;
ideguti 1:7b9e3032bb7b 140 }
ShotaroSenzaki 3:4d19e16fdf4c 141
ideguti 0:0805c5a1b328 142 serial_counter ++;
ideguti 0:0805c5a1b328 143
yuji8822 5:6f6948b8056b 144 printf("motor1=%d,motor2=%d,motor3=%d,motor4=%d,x=%d,y=%d\n\r",motor1,motor2,motor3,motor4,LSX-128,128-LSY);
yuji8822 5:6f6948b8056b 145 //printf("motor1=%d,motor2=%d,motor3=%d,motor4=%d\n\r",motor1,motor2,motor3,motor4);
yuji8822 5:6f6948b8056b 146 //printf("x=%d,y=%d\n\r",LSX-128,128-LSY);
ideguti 0:0805c5a1b328 147
ideguti 0:0805c5a1b328 148 #ifdef DEBUG_MODE
ideguti 0:0805c5a1b328 149 if(serial_counter > REFRESHRATE_PRINTF){
ideguti 0:0805c5a1b328 150 printf("angle %f",angle*180/M_PI);
ideguti 0:0805c5a1b328 151 printf("\t avr_accel_x %f",avr_accel_x);
ideguti 0:0805c5a1b328 152 printf("\t math1 %f",math1);
ideguti 0:0805c5a1b328 153 printf("\t c_accel %f",c_accel);
ideguti 0:0805c5a1b328 154 //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
ideguti 0:0805c5a1b328 155 printf("\t kitai_angle %f",kitai_angle*180/M_PI);
ideguti 0:0805c5a1b328 156 printf("\n");
ideguti 0:0805c5a1b328 157
ideguti 0:0805c5a1b328 158
ideguti 0:0805c5a1b328 159 printf("data[0] %f",val[0]);
ideguti 0:0805c5a1b328 160 printf("\t");
ideguti 0:0805c5a1b328 161 printf("data[1] %f",val[1]);
ideguti 0:0805c5a1b328 162 printf("\t");
ideguti 0:0805c5a1b328 163 printf("data[2] %f",val[2]);
ideguti 0:0805c5a1b328 164 printf("\t");
ideguti 0:0805c5a1b328 165 printf("data[3] %f",val[3]);
ideguti 0:0805c5a1b328 166 printf("\n");
ideguti 0:0805c5a1b328 167
ideguti 0:0805c5a1b328 168 serial_counter = 0;
ideguti 0:0805c5a1b328 169 }
ideguti 0:0805c5a1b328 170 #endif
ideguti 0:0805c5a1b328 171
ideguti 0:0805c5a1b328 172
ideguti 0:0805c5a1b328 173
ideguti 0:0805c5a1b328 174
ideguti 0:0805c5a1b328 175
ideguti 0:0805c5a1b328 176 #endif
ideguti 0:0805c5a1b328 177
ideguti 0:0805c5a1b328 178
ideguti 0:0805c5a1b328 179
ideguti 0:0805c5a1b328 180 }