NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

ハセオムニのプログラム

Dependencies:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@24:58e3ac686b05, 2021-01-24 (annotated)

Committer:
LVRhase01
Date:
Sun Jan 24 04:19:49 2021 +0000
Revision:
24:58e3ac686b05
Parent:
17:48b94e5b31e8
Child:
25:0e4817d84fd5
ver RCWContoroller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LVRhase01 1:3d8552eac4fa 1 #define BEAT 140
LVRhase01 0:b3c48e055e7b 2 #include "mbed.h"
LVRhase01 1:3d8552eac4fa 3 #include "math.h"
LVRhase01 1:3d8552eac4fa 4 #include "omni_wheel.h"
LVRhase01 9:1ea57af922e7 5 #include "motorsmlap.h"
LVRhase01 9:1ea57af922e7 6 #include "jy901.h"
LVRhase01 9:1ea57af922e7 7 #include "SerialMultiByte.h"
LVRhase01 9:1ea57af922e7 8 #include "PID.h"
LVRhase01 9:1ea57af922e7 9
LVRhase01 9:1ea57af922e7 10 JY901 jy(PB_9, PB_8); //sda, scl
LVRhase01 1:3d8552eac4fa 11
LVRhase01 9:1ea57af922e7 12 PID angle(2.0,0,0,0.01);
LVRhase01 9:1ea57af922e7 13 Serial pc(USBTX, USBRX, 115200);
LVRhase01 1:3d8552eac4fa 14
LVRhase01 1:3d8552eac4fa 15 PwmOut beep(PA_0);
LVRhase01 1:3d8552eac4fa 16 DigitalOut led1(PA_11);
LVRhase01 1:3d8552eac4fa 17 DigitalOut led2(LED2);
LVRhase01 24:58e3ac686b05 18 SerialMultiByte arduino(PC_10,PC_11);
LVRhase01 1:3d8552eac4fa 19
LVRhase01 9:1ea57af922e7 20 const double PII = 3.14159265358979;
LVRhase01 9:1ea57af922e7 21 int stick[4],trigger[2];
LVRhase01 9:1ea57af922e7 22 int b[12],b2[12],b3[12];
LVRhase01 9:1ea57af922e7 23 float norm[2];
LVRhase01 9:1ea57af922e7 24
LVRhase01 9:1ea57af922e7 25 OmniWheel omni(3);
LVRhase01 9:1ea57af922e7 26
LVRhase01 2:5a55f555e885 27 motorSmLap motor[] = {
LVRhase01 2:5a55f555e885 28 motorSmLap(PC_6, PC_9, PC_8),
LVRhase01 2:5a55f555e885 29 motorSmLap(PB_6, PA_9, PC_7),
LVRhase01 9:1ea57af922e7 30 motorSmLap(PA_8, PB_4, PB_7)
LVRhase01 2:5a55f555e885 31 };
LVRhase01 9:1ea57af922e7 32
LVRhase01 9:1ea57af922e7 33
LVRhase01 1:3d8552eac4fa 34 void setup();
LVRhase01 1:3d8552eac4fa 35
LVRhase01 1:3d8552eac4fa 36 void setup(){
LVRhase01 24:58e3ac686b05 37 omni.wheel[0].setRadian(PII/4.0);
LVRhase01 24:58e3ac686b05 38 omni.wheel[1].setRadian(3.0/4.0*PII);
LVRhase01 24:58e3ac686b05 39 omni.wheel[2].setRadian(3.0/2.0*PII);
LVRhase01 1:3d8552eac4fa 40
LVRhase01 1:3d8552eac4fa 41 beep.period(1.0/3136);
LVRhase01 1:3d8552eac4fa 42 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 43 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 44
LVRhase01 1:3d8552eac4fa 45 beep.period(1.0/2960);
LVRhase01 1:3d8552eac4fa 46 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 47 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 48
LVRhase01 1:3d8552eac4fa 49 beep.period(1.0/2489);
LVRhase01 1:3d8552eac4fa 50 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 51 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 52
LVRhase01 1:3d8552eac4fa 53 beep.period(1.0/1760);
LVRhase01 1:3d8552eac4fa 54 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 55 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 56
LVRhase01 1:3d8552eac4fa 57 beep.period(1.0/1661);
LVRhase01 1:3d8552eac4fa 58 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 59 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 60
LVRhase01 1:3d8552eac4fa 61 beep.period(1.0/2637);
LVRhase01 1:3d8552eac4fa 62 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 63 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 64
LVRhase01 1:3d8552eac4fa 65 beep.period(1.0/3322);
LVRhase01 1:3d8552eac4fa 66 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 67 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 68
LVRhase01 1:3d8552eac4fa 69 beep.period(1.0/4186);
LVRhase01 1:3d8552eac4fa 70 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 71 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 72
LVRhase01 1:3d8552eac4fa 73 beep.write(0.0f);
LVRhase01 1:3d8552eac4fa 74 wait_ms(BEAT);
LVRhase01 0:b3c48e055e7b 75 }
LVRhase01 0:b3c48e055e7b 76
LVRhase01 1:3d8552eac4fa 77 int main() {
LVRhase01 1:3d8552eac4fa 78 setup();
LVRhase01 9:1ea57af922e7 79 float x, y,now_angle;
LVRhase01 9:1ea57af922e7 80 jy.calibrateAll(50);
LVRhase01 9:1ea57af922e7 81 angle.setInputLimits(-180.0,180.0);
LVRhase01 9:1ea57af922e7 82 angle.setOutputLimits(-1,1);
LVRhase01 9:1ea57af922e7 83 angle.setBias(0);
LVRhase01 9:1ea57af922e7 84 angle.setMode(1);
LVRhase01 9:1ea57af922e7 85 angle.setSetPoint(0);
LVRhase01 17:48b94e5b31e8 86 arduino.setHeaders(127,127);
LVRhase01 24:58e3ac686b05 87 arduino.startReceive(9);
LVRhase01 24:58e3ac686b05 88 uint8_t data[10];
LVRhase01 17:48b94e5b31e8 89
LVRhase01 1:3d8552eac4fa 90 while (true) {
LVRhase01 9:1ea57af922e7 91 led2=1;
LVRhase01 17:48b94e5b31e8 92 arduino.getData(data);
LVRhase01 24:58e3ac686b05 93 y=(data[3]-127.5)/127.5;
LVRhase01 17:48b94e5b31e8 94 x=(data[2]-127.5)/127.5;
LVRhase01 24:58e3ac686b05 95 //誤差をなくす
LVRhase01 24:58e3ac686b05 96 if(x==-1 && y==-1){//
LVRhase01 24:58e3ac686b05 97 x=0;
LVRhase01 24:58e3ac686b05 98 y=0;
LVRhase01 24:58e3ac686b05 99 }
LVRhase01 24:58e3ac686b05 100 if((x>=0&&x<=0.2)||(x<=0&&x>=-0.2)){
LVRhase01 2:5a55f555e885 101 x=0;
LVRhase01 2:5a55f555e885 102 }
LVRhase01 2:5a55f555e885 103
LVRhase01 24:58e3ac686b05 104 if((y>=0&&y<=0.2)||(y<=0&&y>=-0.2)){
LVRhase01 2:5a55f555e885 105 y=0;
LVRhase01 2:5a55f555e885 106 }
LVRhase01 24:58e3ac686b05 107 //十字モード
LVRhase01 24:58e3ac686b05 108 if(data[1]==1){
LVRhase01 24:58e3ac686b05 109 y=0.5;
LVRhase01 24:58e3ac686b05 110 }
LVRhase01 24:58e3ac686b05 111 if(data[1]==2){
LVRhase01 24:58e3ac686b05 112 y=-0.5;
LVRhase01 24:58e3ac686b05 113 }
LVRhase01 24:58e3ac686b05 114 if(data[1]==4){
LVRhase01 24:58e3ac686b05 115 x=0.5;
LVRhase01 24:58e3ac686b05 116 }
LVRhase01 24:58e3ac686b05 117 if(data[1]==8){
LVRhase01 24:58e3ac686b05 118 x=-0.5;
LVRhase01 24:58e3ac686b05 119 }
LVRhase01 24:58e3ac686b05 120 //iosジャイロモード
LVRhase01 24:58e3ac686b05 121 if(data[0]==10){//L1とR1が同時に押された時の値
LVRhase01 24:58e3ac686b05 122 if(data[8]<30){
LVRhase01 24:58e3ac686b05 123 y=0.5;
LVRhase01 24:58e3ac686b05 124 }
LVRhase01 24:58e3ac686b05 125 if(data[8]>230){
LVRhase01 24:58e3ac686b05 126 y=-0.5;
LVRhase01 24:58e3ac686b05 127 }
LVRhase01 24:58e3ac686b05 128 if(data[6]<40){
LVRhase01 24:58e3ac686b05 129 x=-0.5;
LVRhase01 24:58e3ac686b05 130 }
LVRhase01 24:58e3ac686b05 131 if(data[6]>220){
LVRhase01 24:58e3ac686b05 132 x=0.5;
LVRhase01 24:58e3ac686b05 133 }
LVRhase01 24:58e3ac686b05 134 }
LVRhase01 9:1ea57af922e7 135 now_angle = jy.getZaxisAngle();
LVRhase01 2:5a55f555e885 136
LVRhase01 9:1ea57af922e7 137 if(now_angle>180 && now_angle<360){
LVRhase01 9:1ea57af922e7 138 now_angle = now_angle-360;
LVRhase01 9:1ea57af922e7 139 }
LVRhase01 9:1ea57af922e7 140 angle.setProcessValue(now_angle);
LVRhase01 1:3d8552eac4fa 141 omni.computeXY(
LVRhase01 1:3d8552eac4fa 142 x,
LVRhase01 1:3d8552eac4fa 143 y,
LVRhase01 1:3d8552eac4fa 144 0,
LVRhase01 1:3d8552eac4fa 145 0,
LVRhase01 9:1ea57af922e7 146 angle.compute()
LVRhase01 9:1ea57af922e7 147
LVRhase01 1:3d8552eac4fa 148 );
LVRhase01 2:5a55f555e885 149 for(int i=0; i<3; i++){
LVRhase01 9:1ea57af922e7 150 motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
LVRhase01 2:5a55f555e885 151 }
LVRhase01 24:58e3ac686b05 152 pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
LVRhase01 1:3d8552eac4fa 153 }
LVRhase01 1:3d8552eac4fa 154 }