敦哉 品川 / Mbed 2 deprecated manual_robot

manual control

Dependencies:   mbed PS3__

main.cpp@2:5c663c430779, 2019-12-25 (annotated)

Committer:
shina
Date:
Wed Dec 25 06:13:07 2019 +0000
Revision:
2:5c663c430779
Parent:
0:52c9292660e4 
manual robot program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shina 0:52c9292660e4 1 //交ロボ nucleo プログラム　 茨城D
shina 0:52c9292660e4 2
shina 0:52c9292660e4 3 //設定
shina 0:52c9292660e4 4 #include "mbed.h"
shina 0:52c9292660e4 5 #include "PS3.h"
shina 0:52c9292660e4 6 I2C i2c(D14,D15);
shina 0:52c9292660e4 7 Serial pc(USBTX,USBRX);
shina 0:52c9292660e4 8 PS3 ps3(D8,D2);
shina 0:52c9292660e4 9 DigitalOut stop(D9);
shina 0:52c9292660e4 10 DigitalOut green(D7);
shina 0:52c9292660e4 11 DigitalOut yellow(D6);
shina 0:52c9292660e4 12 DigitalOut red(D5);
shina 0:52c9292660e4 13
shina 0:52c9292660e4 14 void initialization();
shina 0:52c9292660e4 15 void emergency();
shina 0:52c9292660e4 16 void get_data();
shina 0:52c9292660e4 17 void change_data();
shina 0:52c9292660e4 18 void send_data(char address,char data);
shina 0:52c9292660e4 19
shina 0:52c9292660e4 20 char data_right=0x00;
shina 0:52c9292660e4 21 char data_left=0x00;
shina 0:52c9292660e4 22 int Ry=0;
shina 0:52c9292660e4 23 int left1=0;
shina 0:52c9292660e4 24 int right1=0;
shina 0:52c9292660e4 25 int start=0;
shina 0:52c9292660e4 26 int old_start=0;
shina 0:52c9292660e4 27 bool flag=1;
shina 0:52c9292660e4 28
shina 0:52c9292660e4 29 int main(){
shina 0:52c9292660e4 30 green=1;
shina 0:52c9292660e4 31 stop=0;
shina 0:52c9292660e4 32 initialization();
shina 0:52c9292660e4 33 while(true){
shina 0:52c9292660e4 34 get_data();
shina 0:52c9292660e4 35 emergency();
shina 0:52c9292660e4 36 change_data();
shina 0:52c9292660e4 37 send_data(0x12,data_right);
shina 0:52c9292660e4 38 send_data(0x14,data_left);
shina 0:52c9292660e4 39 }
shina 0:52c9292660e4 40 }
shina 0:52c9292660e4 41
shina 0:52c9292660e4 42 void initialization(){
shina 0:52c9292660e4 43 red=0;
shina 0:52c9292660e4 44 data_right=0x00;
shina 0:52c9292660e4 45 data_left=0x00;
shina 0:52c9292660e4 46 send_data(0x12,data_right);
shina 0:52c9292660e4 47 send_data(0x14,data_left);
shina 0:52c9292660e4 48 }
shina 0:52c9292660e4 49
shina 0:52c9292660e4 50 void emergency(){
shina 0:52c9292660e4 51 if(old_start!=start){
shina 0:52c9292660e4 52 old_start=start;
shina 0:52c9292660e4 53 if(start==1){
shina 0:52c9292660e4 54 if(flag==1){
shina 0:52c9292660e4 55 stop=1;
shina 0:52c9292660e4 56 flag=0;
shina 0:52c9292660e4 57 red=1;
shina 0:52c9292660e4 58 }else if(flag==0){
shina 0:52c9292660e4 59 stop=0;
shina 0:52c9292660e4 60 flag=1;
shina 0:52c9292660e4 61 red=0;
shina 0:52c9292660e4 62 }
shina 0:52c9292660e4 63 }
shina 0:52c9292660e4 64 }
shina 0:52c9292660e4 65 }
shina 0:52c9292660e4 66
shina 0:52c9292660e4 67 void get_data(){
shina 0:52c9292660e4 68 Ry=ps3.getLeftJoystickYaxis();
shina 0:52c9292660e4 69 left1=ps3.getButtonState(L1);
shina 0:52c9292660e4 70 right1=ps3.getButtonState(R1);
shina 0:52c9292660e4 71 start=ps3.getSTARTState();
shina 0:52c9292660e4 72 }
shina 0:52c9292660e4 73
shina 0:52c9292660e4 74 void change_data(){
shina 0:52c9292660e4 75 if(Ry>32&&right1==0&&left1==0){
shina 0:52c9292660e4 76 red=1;
shina 0:52c9292660e4 77 data_right=0xff;
shina 0:52c9292660e4 78 data_left=0xaf;
shina 0:52c9292660e4 79 }else if(Ry<-32&&right1==0&&left1==0){
shina 0:52c9292660e4 80 red=1;
shina 0:52c9292660e4 81 data_right=0xaf;
shina 0:52c9292660e4 82 data_left=0xff;
shina 0:52c9292660e4 83 }else if(-32<=Ry<=32&&right1==1&&left1==0){
shina 0:52c9292660e4 84 red=1;
shina 0:52c9292660e4 85 data_right=0xff;
shina 0:52c9292660e4 86 data_left=0xff;
shina 0:52c9292660e4 87 }else if(-32<=Ry<=32&&right1==0&&left1==1){
shina 0:52c9292660e4 88 red=1;
shina 0:52c9292660e4 89 data_right=0xaf;
shina 0:52c9292660e4 90 data_left=0xaf;
shina 0:52c9292660e4 91 }else{
shina 0:52c9292660e4 92 red=0;
shina 0:52c9292660e4 93 data_right=0x00;
shina 0:52c9292660e4 94 data_left=0x00;
shina 0:52c9292660e4 95 }
shina 0:52c9292660e4 96
shina 0:52c9292660e4 97 }
shina 0:52c9292660e4 98
shina 0:52c9292660e4 99 void send_data(char address,char data){
shina 0:52c9292660e4 100 i2c.start();
shina 0:52c9292660e4 101 yellow=i2c.write(address);
shina 0:52c9292660e4 102 i2c.write(data);
shina 0:52c9292660e4 103 i2c.stop();
shina 0:52c9292660e4 104 wait(0.003);
shina 0:52c9292660e4 105
shina 0:52c9292660e4 106 }