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@4:ee01e28db9d4, 2015-10-15 (annotated)

Committer:
ShotaroSenzaki
Date:
Thu Oct 15 12:30:50 2015 +0000
Revision:
4:ee01e28db9d4
Parent:
3:4d19e16fdf4c
Child:
5:6f6948b8056b
aaa

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
ShotaroSenzaki 4:ee01e28db9d4 61 int motor1=0;
ShotaroSenzaki 4:ee01e28db9d4 62 int motor2=0;
ShotaroSenzaki 4:ee01e28db9d4 63 int motor3=0;
ShotaroSenzaki 4:ee01e28db9d4 64 int motor4=0;
ideguti 0:0805c5a1b328 65
ideguti 0:0805c5a1b328 66 //
ideguti 0:0805c5a1b328 67 u8 RSX,RSY,LSX,LSY,BSU,BSL;
ideguti 0:0805c5a1b328 68 //コントローラ断線時対策用
ideguti 0:0805c5a1b328 69 u8 accel[5];
ideguti 0:0805c5a1b328 70 u8 accel_nowsub=0;
ideguti 0:0805c5a1b328 71 //////////
ideguti 0:0805c5a1b328 72 char cnt=0;
ideguti 0:0805c5a1b328 73
ideguti 0:0805c5a1b328 74
ideguti 0:0805c5a1b328 75 void UserLoopSetting(){
ShotaroSenzaki 3:4d19e16fdf4c 76 can1.frequency(500000);
ideguti 0:0805c5a1b328 77 }
ideguti 0:0805c5a1b328 78
ideguti 0:0805c5a1b328 79 void UserLoop(char n,const u8* data){
ideguti 0:0805c5a1b328 80 //PS3conSTATE--->value
ideguti 0:0805c5a1b328 81 u16 ButtonState;
ideguti 1:7b9e3032bb7b 82 char Send_data[8] = {};
ShotaroSenzaki 4:ee01e28db9d4 83 //u16 spi_send_data[10] = {};
ideguti 0:0805c5a1b328 84 if(n==0){//有線Ps3USB.cpp
ideguti 0:0805c5a1b328 85 RSX = ((ps3report*)data)->RightStickX;
ideguti 0:0805c5a1b328 86 RSY = ((ps3report*)data)->RightStickY;
ideguti 0:0805c5a1b328 87 LSX = ((ps3report*)data)->LeftStickX;
ideguti 0:0805c5a1b328 88 LSY = ((ps3report*)data)->LeftStickY;
ideguti 0:0805c5a1b328 89 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 90 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
ideguti 0:0805c5a1b328 91 //ボタンの処理
ideguti 0:0805c5a1b328 92 ButtonState = ((ps3report*)data)->ButtonState;
ideguti 0:0805c5a1b328 93 }else {//無線TestShell.cpp
ideguti 0:0805c5a1b328 94 RSX = ((ps3report*)(data + 1))->RightStickX;
ideguti 0:0805c5a1b328 95 RSY = ((ps3report*)(data + 1))->RightStickY;
ideguti 0:0805c5a1b328 96 LSX = ((ps3report*)(data + 1))->LeftStickX;
ideguti 0:0805c5a1b328 97 LSY = ((ps3report*)(data + 1))->LeftStickY;
ideguti 0:0805c5a1b328 98 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 99 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
ideguti 0:0805c5a1b328 100 //ボタンの処理
ideguti 0:0805c5a1b328 101 ButtonState = ((ps3report*)(data + 1))->ButtonState;
ideguti 0:0805c5a1b328 102 }
ShotaroSenzaki 4:ee01e28db9d4 103
ShotaroSenzaki 4:ee01e28db9d4 104 motor1 = (M_MAX/180)*((LSX-128)/sqrt(2)+((LSY-128)/sqrt(2));
ShotaroSenzaki 4:ee01e28db9d4 105 motor2 = (M_MAX/180)*(-(LSX-128)/sqrt(2)+((LSY-128)/sqrt(2));
ShotaroSenzaki 4:ee01e28db9d4 106 motor3 = motor1*-1;
ShotaroSenzaki 4:ee01e28db9d4 107 motor4 = motor2*-1;
ShotaroSenzaki 4:ee01e28db9d4 108
ShotaroSenzaki 4:ee01e28db9d4 109 //Send_data[0] = RSX; //MAX 1000 MIN -1000 MD MAX 4096 MIN -4096
ShotaroSenzaki 4:ee01e28db9d4 110 Send_data[0] = motor1 >>8;
ShotaroSenzaki 4:ee01e28db9d4 111 Send_data[1] = motor1 &&255;
ShotaroSenzaki 4:ee01e28db9d4 112 Send_data[2] = motor2 >>8;
ShotaroSenzaki 4:ee01e28db9d4 113 Send_data[3] = motor2 &&255;
ShotaroSenzaki 4:ee01e28db9d4 114 send_data[4] = motor3 >>8;
ShotaroSenzaki 4:ee01e28db9d4 115 Send_data[5] = motor3 &&255;
ShotaroSenzaki 4:ee01e28db9d4 116 Send_data[6] = motor4 >>8;
ShotaroSenzaki 4:ee01e28db9d4 117 Send_data[7] = motor4 &&255;
ShotaroSenzaki 4:ee01e28db9d4 118 can1.write(CANMessage(1, &Send_data[0], 8)); //id=1 自動　id=23 手動
ideguti 1:7b9e3032bb7b 119
ideguti 1:7b9e3032bb7b 120 static int can1_counter = 0;
ideguti 1:7b9e3032bb7b 121 can1_counter ++;
ideguti 1:7b9e3032bb7b 122 if(can1_counter > 100){
ideguti 1:7b9e3032bb7b 123 led1 = !led1;
ideguti 1:7b9e3032bb7b 124 can1_counter = 0;
ideguti 1:7b9e3032bb7b 125 }
ShotaroSenzaki 3:4d19e16fdf4c 126
ideguti 0:0805c5a1b328 127 serial_counter ++;
ideguti 0:0805c5a1b328 128
ShotaroSenzaki 4:ee01e28db9d4 129 printf("%d,%d,%d,%d,%f\n",Send_data[0],Send_data[1],RSX-128,128-RSY,theta2-M_PI/4);
ideguti 0:0805c5a1b328 130
ideguti 0:0805c5a1b328 131 #ifdef DEBUG_MODE
ideguti 0:0805c5a1b328 132 if(serial_counter > REFRESHRATE_PRINTF){
ideguti 0:0805c5a1b328 133 printf("angle %f",angle*180/M_PI);
ideguti 0:0805c5a1b328 134 printf("\t avr_accel_x %f",avr_accel_x);
ideguti 0:0805c5a1b328 135 printf("\t math1 %f",math1);
ideguti 0:0805c5a1b328 136 printf("\t c_accel %f",c_accel);
ideguti 0:0805c5a1b328 137 //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
ideguti 0:0805c5a1b328 138 printf("\t kitai_angle %f",kitai_angle*180/M_PI);
ideguti 0:0805c5a1b328 139 printf("\n");
ideguti 0:0805c5a1b328 140
ideguti 0:0805c5a1b328 141
ideguti 0:0805c5a1b328 142 printf("data[0] %f",val[0]);
ideguti 0:0805c5a1b328 143 printf("\t");
ideguti 0:0805c5a1b328 144 printf("data[1] %f",val[1]);
ideguti 0:0805c5a1b328 145 printf("\t");
ideguti 0:0805c5a1b328 146 printf("data[2] %f",val[2]);
ideguti 0:0805c5a1b328 147 printf("\t");
ideguti 0:0805c5a1b328 148 printf("data[3] %f",val[3]);
ideguti 0:0805c5a1b328 149 printf("\n");
ideguti 0:0805c5a1b328 150
ideguti 0:0805c5a1b328 151 serial_counter = 0;
ideguti 0:0805c5a1b328 152 }
ideguti 0:0805c5a1b328 153 #endif
ideguti 0:0805c5a1b328 154
ideguti 0:0805c5a1b328 155
ideguti 0:0805c5a1b328 156
ideguti 0:0805c5a1b328 157
ideguti 0:0805c5a1b328 158
ideguti 0:0805c5a1b328 159 #endif
ideguti 0:0805c5a1b328 160
ideguti 0:0805c5a1b328 161
ideguti 0:0805c5a1b328 162
ideguti 0:0805c5a1b328 163 }