春ロボ1班(元F3RC4班+)
さいしんばん
Dependencies: mbed move3wheel
Fork of
F3RC-mbed-new4
by 
春ロボ1班(元F3RC4班+)
Diff: User.cpp
- Revision:
- 16:55c180a4338c
- Parent:
- 15:c5258a4dee75
- Child:
- 17:95cb86ce56a9
--- a/User.cpp Sat Apr 29 11:48:36 2017 +0000
+++ b/User.cpp Tue Sep 04 12:10:13 2018 +0000
@@ -5,22 +5,60 @@
#include "User.h"
#include "mbed.h"
+#include "math.h"
+#include "move3wheel.h"
+
int RSX,RSY,LSX,LSY,BSU,BSL;
-//これより下に関数外に書く要素を記入する
-PwmOut led1(LED1);
-PwmOut led2(LED2);
+/*DigitalOut led1(LED1);
+DigitalOut led2(LED2);
DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+DigitalOut led4(LED4);*/
+
+SPI spi(p5,p6,p7);
+DigitalOut cs(p8);
+int data1 = 0;
+int a; //aは定数
+double receive_data/*,true_data*/;
+double now_angle,target_angle,out;
-void UserLoopSetting(){
- //一度だけ行いたい初期設定をここに書く
- led1.period_ms(20);
+PwmOut motor0CW(p21); //0番motor 外から見て時計回り
+PwmOut motor0CCW(p22); //0番motor 反時計回り
+PwmOut motor1CW(p23); //1番motor
+PwmOut motor1CCW(p24); //1番motor
+PwmOut motor2CW(p26); //2番motor
+PwmOut motor2CCW(p25); //2番motor
+
+
+void motor(double cw0,double ccw0,double cw1,double ccw1,double cw2,double ccw2)
+{
+ motor0CW = cw0;
+ motor0CCW = ccw0;
+ motor1CW = cw1;
+ motor1CCW = ccw1;
+ motor2CW = cw2;
+ motor2CCW = ccw2;
}
-void UserLoop(char n,const u8* data){
+
+
+void UserLoopSetting()
+{
+ spi.format(16,3);
+ spi.frequency(1000000);
+
+ motor0CW.period_us(50);
+ motor0CCW.period_us(50);
+ motor1CW.period_us(50);
+ motor1CCW.period_us(50);
+ motor2CW.period_us(50);
+ motor2CCW.period_us(50);
+}
+
+void UserLoop(char n,const u8* data)
+{
u16 ButtonState;
- if(n==0){//有線Ps3USB.cpp
+ if(n==0) { //有線Ps3USB.cpp
RSX = ((ps3report*)data)->RightStickX;
RSY = ((ps3report*)data)->RightStickY;
LSX = ((ps3report*)data)->LeftStickX;
@@ -29,7 +67,7 @@
BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
//ボタンの処理
ButtonState = ((ps3report*)data)->ButtonState;
- }else {//無線TestShell.cpp
+ } else {//無線TestShell.cpp
RSX = ((ps3report*)(data + 1))->RightStickX;
RSY = ((ps3report*)(data + 1))->RightStickY;
LSX = ((ps3report*)(data + 1))->LeftStickX;
@@ -39,25 +77,398 @@
//ボタンの処理
ButtonState = ((ps3report*)(data + 1))->ButtonState;
}
- //ここより下にプログラムを書く
+
+///////////////////////////ここからmotorのプログラム///////////////////////////////
+
+ /*int a = (ButtonState >> BUTTONRIGHT)&1;
+ if(a == 0)led4=0;
+ else led4=1;*/
+
+ //printf("RSX = %d, RSY = %d\r\n", RSX, RSY);
+ if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150
+ && (((ButtonState >> BUTTONRIGHT)&1) == 0) && (((ButtonState >> BUTTONDOWN)&1) == 0)) { //ニュートラルポジション
+ motor(0,0,0,0,0,0);
+ /*led1 = 1;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;*/
+
+
+ } else if(RSX >=100 && RSX <150 && RSY <100) { // ↑ 移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0,0.13,0);
+
+ }else{
+ CalMotorOut(0,0.3,0); //走らせたい速さを入れる(X方向,y方向,回転) 回転方向は上から見て反時計回りが正
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0);
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.13,0,0);
+
+ }else{
+ CalMotorOut(0.3,0,0);
+ }
+ motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
+
+ }else{
+ motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); //1番motorはCW方向、2番motorはCCW方向に回す
+
+ }
+
+ } else if(RSX >=100 && RSX <150 && RSY >=150) { // ↓ 移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0,0.13,0);
+
+ }else{
+ CalMotorOut(0,0.3,0);
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.13,0,0);
+
+ }else{
+ CalMotorOut(0.3,0,0);
+ }
+ motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
+
+ }else{
+ motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); //前進と逆の方向に回す
- //データ取得例
- if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
- led1 = 1;
- //○が押されたとき
- }else{
- led1 = 0;
- //○を押してないとき
+ }
+
+ } else if(RSX >=150 && RSY >=100 && RSY <150) { // → 移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.13,0,0);
+
+ }else{
+ CalMotorOut(0.3,0,0);
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0,0.13,0);
+
+ }else{
+ CalMotorOut(0,0.3,0);
+ }
+ motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));
+
+ }else{
+ motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
+
+ }
+
+ } else if(RSX <100 && RSY >=100 && RSY <150) { // ← 移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.13,0,0);
+
+ }else{
+ CalMotorOut(0.3,0,0);
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0,0.13,0);
+
+ }else{
+ CalMotorOut(0,0.3,0);
+ }
+ motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0);
+
+ }else{
+ motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
+
+ }
+
+ } else if(RSX < 100 && RSY <100) { // 左上移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.1,0.1,0);
+
+ }else{
+ CalMotorOut(0.2,0.2,0);
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(GetMotorOut(0),0,0,GetMotorOut(1),-GetMotorOut(2),0);
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.1,-0.1,0);
+
+ }else{
+ CalMotorOut(0.2,-0.2,0);
+ }
+ motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0);
+
+ }else{
+ motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2));
+
+ }
+
+ } else if(RSX >= 150 && RSY <100) { // 右上移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(-0.1,0.1,0);
+
+ }else{
+ CalMotorOut(-0.2,0.2,0);
}
- led1=LSX/256.0f;
- led2=LSY/256.0f;
- led3=ButtonState & 0x0400; //L1の状態
- led4=ButtonState & 0x0800; //R1の状態
- //値の取得はps3.hを参照
- //ここまでプログラム例 実機に乗せるときは消して大丈夫です
+ if(now_angle > 135 && now_angle < 225){
+ motor(0,-GetMotorOut(0),0,GetMotorOut(1),-GetMotorOut(2),0);
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.1,0.1,0);
+
+ }else{
+ CalMotorOut(0.2,0.2,0);
+ }
+ motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2));
+
+ }else{
+ motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2));
+
+ }
+
+ } else if(RSX <100 && RSY >=150) { // 左下移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(0.1,-0.1,0);
+
+ }else{
+ CalMotorOut(0.2,-0.2,0);
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(GetMotorOut(0),0,-GetMotorOut(1),0,0,GetMotorOut(2));
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(-0.1,-0.1,0);
+
+ }else{
+ CalMotorOut(-0.2,-0.2,0);
+ }
+ motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0);
+
+ }else{
+ motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0);
+
+ }
+
+ } else if(RSX >= 150 && RSY >=150) { // 右下移動
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(-0.1,-0.1,0);
+
+ }else{
+ CalMotorOut(-0.2,-0.2,0);
+ }
+
+ if(now_angle > 135 && now_angle < 225){
+ motor(0,-GetMotorOut(0),-GetMotorOut(1),0,0,GetMotorOut(2));
+
+ }else if(now_angle > 225 && now_angle < 315){
+ if((ButtonState >> BUTTONR1)&1 == 1){
+ CalMotorOut(-0.1,-0.1,0);
+
+ }else{
+ CalMotorOut(-0.2,0.2,0);
+ }
+ motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2));
+
+ }else{
+ motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0);
+
+ }
+
+ }
+ if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150) { //右スティックニュートラルポジション
+ if(LSX >=100 && LSX <150 ) { //左スティックニュートラルポジション
+ motor(0,0,0,0,0,0);
+
+ } else if(LSX >=150 ) { //右回転
+ motor(0,0.2,0,0.2,0,0.2);
+
+ } else if(LSX <100 ) { //左回転
+ motor(0.2,0,0.2,0,0.2,0);
+
+ } else {
+ motor(0,0,0,0,0,0);
+ }
+ }
+////////////////////////////ここからヌクレオのプログラム/////////////////////////////
+
+ if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //servo1
+ wait(0.1);
+ if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {
+ data1=1;
+
+ }
+ }
+
+
+ if((ButtonState >> BUTTONCIRCLE)&1 == 1) { //servo2
+ wait(0.1);
+ if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
+ data1=2;
+ }
+ }
+
+
+ if((ButtonState >> BUTTONCROSS)&1 == 1) { //servo3
+ wait(0.1);
+ if((ButtonState >> BUTTONCROSS)&1 == 1) {
+ data1=3;
+ }
+ }
+
+
+ if((ButtonState >> BUTTONSQUARE)&1 == 1) { //servo4
+ wait(0.1);
+ if((ButtonState >> BUTTONSQUARE)&1 == 1) {
+ data1=4;
+ }
+ }
+
+
+ if((ButtonState >> BUTTONLEFT)&1 == 1) { //エアシリンダー
+ wait(0.1);
+ if((ButtonState >> BUTTONLEFT)&1 == 1) {
+ data1=5;
+ }
+ }
+
+////////////////////////ここからジャイロの角度指定プログラム//////////////////////////
+
+ if(receive_data >= 0) { //反時計回りに0~359°となるよう修正
+ a = receive_data / 360;
+ now_angle = receive_data - (360 * a); //現在の角度
+ } else {
+ a = receive_data / 360;
+ now_angle = - receive_data + (360 * a);
+ }
+
+
+ /*true_data = receive_data - 65536;
+ } else {
+ true_data = receive_data;
+ }*/
+
+
+ //now_angle = true_data; //現在の角度
+ //out = 0.01 * (target_angle - now_angle); //P(目標の角度 - 現在の角度) Pは自分で決めた定数
+
+ if((ButtonState >> BUTTONRIGHT)&1 == 1) {
+ wait(0.1);
+ if((ButtonState >> BUTTONRIGHT)&1 == 1) {
+ target_angle = 270;
+ out = 0.01 * (target_angle - now_angle);
+
+ //printf("%f\r\n",now_angle);
+ //printf("%f\r\n",out);
+ if(now_angle >= 268 && now_angle <= 272){
+ motor(0,0,0,0,0,0);
+
+ } else if(out > 1.8 || out <= -0.3) { //0～89°と270～359°のときは時計回りに回転
+ motor(0.3,0,0.3,0,0.3,0);
+ //printf("cw 0.3\r\n");
+
+ } else if(out > -0.3 && out <= 0) {
+ motor(-out,0,-out,0,-out,0);
+ //printf("cw -out\r\n");
+
+ } else if(out > 0 && out <= 0.3) { //90～269°のときは反時計回りに回転
+ motor(0,out,0,out,0,out);
+ //printf("ccw out\r\n");
+
+ } else if(out <= 1.8 && out > 0.3) {
+ motor(0,0.3,0,0.3,0,0.3);
+ //printf("ccw 0.3\r\n");
+ }
+ }
+ }
+
+ if((ButtonState >> BUTTONDOWN)&1 == 1) {
+ wait(0.1);
+ if((ButtonState >> BUTTONDOWN)&1 == 1) {
+ target_angle = 180;
+ out = 0.01 * (target_angle - now_angle);
+
+ if(now_angle >= 178 && now_angle <= 182){
+ motor(0,0,0,0,0,0);
+
+ } else if(out > 0.3) { //0～178°のときは反時計回りに回転
+ motor(0,0.3,0,0.3,0,0.3);
+
+ } else if(out <= 0.3 && out > 0) {
+ motor(0,out,0,out,0,out);
+
+ } else if(out <= 0 && out > -0.3) { //180～359°のときは時計回りに回転
+ motor(-out,0,-out,0,-out,0);
+
+ } else if(out <= -0.3) {
+ motor(0.3,0,0.3,0,0.3,0);
+ }
+ }
+ }
+ if((ButtonState >> BUTTONUP)&1 == 1) {
+ wait(0.1);
+ if((ButtonState >> BUTTONUP)&1 == 1) {
+ if(0 <= now_angle && now_angle < 180) {
+ target_angle = 0;
+ out = -0.01 * (target_angle - now_angle);
+
+ if(now_angle <= 1) {
+ motor(0,0,0,0,0,0);
+
+ } else if(out > 0.3) { //0～179°のときは時計回りに回転
+ motor(0.3,0,0.3,0,0.3,0);
+
+ } else if(out <= 0.3 && out > 0) {
+ motor(out,0,out,0,out,0);
+ }
+ }
+
+ if(180 <= now_angle && now_angle< 360) {
+ target_angle = 360;
+ out = 0.01 * (target_angle - now_angle);
+
+ if(now_angle >= 359) {
+ motor(0,0,0,0,0,0);
+
+ } else if(out > 0.3) { //180～359°のときは反時計回りに回転
+ motor(0,0.3,0,0.3,0,0.3);
+
+ } else if(out <= 0.3 && out > 0) {
+ motor(0,out,0,out,0,out);
+ }
+ }
+ }
+ }
+
+ cs=0;
+ receive_data = spi.write(data1);
+
+ cs=1;
+ data1=0;
+
+
+
}
\ No newline at end of file