NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

ハセオムニのプログラム

Dependencies:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@26:223339e60e00, 2021-07-23 (annotated)

Committer:
LVRhase01
Date:
Fri Jul 23 02:16:43 2021 +0000
Revision:
26:223339e60e00
Parent:
25:0e4817d84fd5
Child:
27:2d7a978e70ab
blue switch mode 0723; wii fit and RCWC controller

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 26:223339e60e00 14 SerialMultiByte arduino(PC_12,PD_2);
LVRhase01 26:223339e60e00 15 SerialMultiByte m5stack(PC_10,PC_11);
LVRhase01 1:3d8552eac4fa 16
LVRhase01 1:3d8552eac4fa 17 PwmOut beep(PA_0);
LVRhase01 26:223339e60e00 18
LVRhase01 26:223339e60e00 19 DigitalIn sw(PC_13);
LVRhase01 26:223339e60e00 20 DigitalOut led1(PB_5);
LVRhase01 1:3d8552eac4fa 21 DigitalOut led2(LED2);
LVRhase01 26:223339e60e00 22 DigitalOut led3(PA_1);
LVRhase01 26:223339e60e00 23 DigitalOut led4(PA_10);
LVRhase01 9:1ea57af922e7 24
LVRhase01 2:5a55f555e885 25 motorSmLap motor[] = {
LVRhase01 2:5a55f555e885 26 motorSmLap(PC_6, PC_9, PC_8),
LVRhase01 2:5a55f555e885 27 motorSmLap(PB_6, PA_9, PC_7),
LVRhase01 9:1ea57af922e7 28 motorSmLap(PA_8, PB_4, PB_7)
LVRhase01 2:5a55f555e885 29 };
LVRhase01 9:1ea57af922e7 30
LVRhase01 26:223339e60e00 31 const double PII = 3.14159265358979;
LVRhase01 26:223339e60e00 32 OmniWheel omni(3);
LVRhase01 9:1ea57af922e7 33
LVRhase01 26:223339e60e00 34 Ticker timer;
LVRhase01 26:223339e60e00 35 int setmode=0;
LVRhase01 26:223339e60e00 36 void tick(void)
LVRhase01 26:223339e60e00 37 {
LVRhase01 26:223339e60e00 38 //pc.printf(" %d\n",setmode);
LVRhase01 26:223339e60e00 39 if(sw.read()==0)led2=1; else led2=0;
LVRhase01 26:223339e60e00 40 }
LVRhase01 1:3d8552eac4fa 41
LVRhase01 1:3d8552eac4fa 42 void setup(){
LVRhase01 26:223339e60e00 43 arduino.setHeaders(127,127);
LVRhase01 26:223339e60e00 44 arduino.startReceive(5);
LVRhase01 26:223339e60e00 45 m5stack.setHeaders(127,127);
LVRhase01 26:223339e60e00 46 m5stack.startReceive(9);
LVRhase01 26:223339e60e00 47
LVRhase01 25:0e4817d84fd5 48 omni.wheel[0].setRadian(PII/2.0);
LVRhase01 25:0e4817d84fd5 49 omni.wheel[1].setRadian(7.0/6.0*PII);
LVRhase01 25:0e4817d84fd5 50 omni.wheel[2].setRadian(11.0/6.0*PII);
LVRhase01 1:3d8552eac4fa 51
LVRhase01 1:3d8552eac4fa 52 beep.period(1.0/3136);
LVRhase01 1:3d8552eac4fa 53 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 54 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 55
LVRhase01 1:3d8552eac4fa 56 beep.period(1.0/2960);
LVRhase01 1:3d8552eac4fa 57 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 58 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 59
LVRhase01 1:3d8552eac4fa 60 beep.period(1.0/2489);
LVRhase01 1:3d8552eac4fa 61 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 62 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 63
LVRhase01 1:3d8552eac4fa 64 beep.period(1.0/1760);
LVRhase01 1:3d8552eac4fa 65 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 66 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 67
LVRhase01 1:3d8552eac4fa 68 beep.period(1.0/1661);
LVRhase01 1:3d8552eac4fa 69 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 70 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 71
LVRhase01 1:3d8552eac4fa 72 beep.period(1.0/2637);
LVRhase01 1:3d8552eac4fa 73 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 74 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 75
LVRhase01 1:3d8552eac4fa 76 beep.period(1.0/3322);
LVRhase01 1:3d8552eac4fa 77 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 78 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 79
LVRhase01 1:3d8552eac4fa 80 beep.period(1.0/4186);
LVRhase01 1:3d8552eac4fa 81 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 82 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 83
LVRhase01 1:3d8552eac4fa 84 beep.write(0.0f);
LVRhase01 1:3d8552eac4fa 85 wait_ms(BEAT);
LVRhase01 0:b3c48e055e7b 86 }
LVRhase01 0:b3c48e055e7b 87
LVRhase01 1:3d8552eac4fa 88 int main() {
LVRhase01 1:3d8552eac4fa 89 setup();
LVRhase01 26:223339e60e00 90 int ssw=1;
LVRhase01 9:1ea57af922e7 91 float x, y,now_angle;
LVRhase01 9:1ea57af922e7 92 jy.calibrateAll(50);
LVRhase01 26:223339e60e00 93 uint8_t weight[5];
LVRhase01 26:223339e60e00 94 uint8_t data[10];
LVRhase01 9:1ea57af922e7 95 angle.setInputLimits(-180.0,180.0);
LVRhase01 9:1ea57af922e7 96 angle.setOutputLimits(-1,1);
LVRhase01 9:1ea57af922e7 97 angle.setBias(0);
LVRhase01 9:1ea57af922e7 98 angle.setMode(1);
LVRhase01 9:1ea57af922e7 99 angle.setSetPoint(0);
LVRhase01 17:48b94e5b31e8 100
LVRhase01 26:223339e60e00 101 sw.mode(PullUp);
LVRhase01 26:223339e60e00 102 timer.attach(&tick, 0.1);
LVRhase01 26:223339e60e00 103
LVRhase01 1:3d8552eac4fa 104 while (true) {
LVRhase01 26:223339e60e00 105
LVRhase01 26:223339e60e00 106 /*青スイッチモード切り替え*/
LVRhase01 26:223339e60e00 107 if(sw.read()==1){
LVRhase01 26:223339e60e00 108 if(ssw==0){
LVRhase01 26:223339e60e00 109 ssw=1;
LVRhase01 24:58e3ac686b05 110 }
LVRhase01 26:223339e60e00 111 }else{
LVRhase01 26:223339e60e00 112 if(ssw==1){
LVRhase01 26:223339e60e00 113 setmode++;
LVRhase01 26:223339e60e00 114 ssw=0;
LVRhase01 26:223339e60e00 115 }
LVRhase01 26:223339e60e00 116 if(setmode>=2){
LVRhase01 26:223339e60e00 117 setmode=0;
LVRhase01 26:223339e60e00 118 }
LVRhase01 2:5a55f555e885 119 }
LVRhase01 2:5a55f555e885 120
LVRhase01 26:223339e60e00 121 switch(setmode){
LVRhase01 26:223339e60e00 122 /*RCWCcontrollerモード*/
LVRhase01 26:223339e60e00 123 case 0:
LVRhase01 26:223339e60e00 124 led1=1;
LVRhase01 26:223339e60e00 125 led3=0;
LVRhase01 26:223339e60e00 126 led4=0;
LVRhase01 26:223339e60e00 127 m5stack.getData(data);
LVRhase01 26:223339e60e00 128 y=(data[3]-127.5)/127.5;
LVRhase01 26:223339e60e00 129 x=(data[2]-127.5)/127.5;
LVRhase01 26:223339e60e00 130 //誤差をなくす
LVRhase01 26:223339e60e00 131 if(x==-1 && y==-1){//
LVRhase01 26:223339e60e00 132 x=0;
LVRhase01 26:223339e60e00 133 y=0;
LVRhase01 26:223339e60e00 134 }
LVRhase01 26:223339e60e00 135 if((x>=0&&x<=0.2)||(x<=0&&x>=-0.2)){
LVRhase01 26:223339e60e00 136 x=0;
LVRhase01 26:223339e60e00 137 }
LVRhase01 26:223339e60e00 138
LVRhase01 26:223339e60e00 139 if((y>=0&&y<=0.2)||(y<=0&&y>=-0.2)){
LVRhase01 26:223339e60e00 140 y=0;
LVRhase01 24:58e3ac686b05 141 }
LVRhase01 26:223339e60e00 142 //十字モード
LVRhase01 26:223339e60e00 143 if(data[1]==1){
LVRhase01 26:223339e60e00 144 y=0.5;
LVRhase01 26:223339e60e00 145 }
LVRhase01 26:223339e60e00 146 if(data[1]==2){
LVRhase01 26:223339e60e00 147 y=-0.5;
LVRhase01 26:223339e60e00 148 }
LVRhase01 26:223339e60e00 149 if(data[1]==4){
LVRhase01 26:223339e60e00 150 x=0.5;
LVRhase01 26:223339e60e00 151 }
LVRhase01 26:223339e60e00 152 if(data[1]==8){
LVRhase01 26:223339e60e00 153 x=-0.5;
LVRhase01 26:223339e60e00 154 }
LVRhase01 26:223339e60e00 155 //iosジャイロモード
LVRhase01 26:223339e60e00 156 if(data[0]==10){//L1とR1が同時に押された時の値
LVRhase01 26:223339e60e00 157 if(data[8]<30){
LVRhase01 26:223339e60e00 158 y=0.5;
LVRhase01 26:223339e60e00 159 }
LVRhase01 26:223339e60e00 160 if(data[8]>230){
LVRhase01 26:223339e60e00 161 y=-0.5;
LVRhase01 26:223339e60e00 162 }
LVRhase01 26:223339e60e00 163 if(data[6]<40){
LVRhase01 26:223339e60e00 164 x=-0.5;
LVRhase01 26:223339e60e00 165 }
LVRhase01 26:223339e60e00 166 if(data[6]>220){
LVRhase01 26:223339e60e00 167 x=0.5;
LVRhase01 26:223339e60e00 168 }
LVRhase01 26:223339e60e00 169 }
LVRhase01 26:223339e60e00 170 now_angle = jy.getZaxisAngle();
LVRhase01 26:223339e60e00 171
LVRhase01 26:223339e60e00 172 if(now_angle>180 && now_angle<360){
LVRhase01 26:223339e60e00 173 now_angle = now_angle-360;
LVRhase01 24:58e3ac686b05 174 }
LVRhase01 26:223339e60e00 175 angle.setProcessValue(now_angle);
LVRhase01 26:223339e60e00 176 omni.computeXY(
LVRhase01 26:223339e60e00 177 x,
LVRhase01 26:223339e60e00 178 y,
LVRhase01 26:223339e60e00 179 0,
LVRhase01 26:223339e60e00 180 0,
LVRhase01 26:223339e60e00 181 angle.compute()
LVRhase01 26:223339e60e00 182
LVRhase01 26:223339e60e00 183 );
LVRhase01 26:223339e60e00 184 for(int i=0; i<3; i++){
LVRhase01 26:223339e60e00 185 motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
LVRhase01 24:58e3ac686b05 186 }
LVRhase01 26:223339e60e00 187 pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
LVRhase01 26:223339e60e00 188 break;
LVRhase01 26:223339e60e00 189
LVRhase01 26:223339e60e00 190 /*wii balance　boardモード*/
LVRhase01 26:223339e60e00 191 case 1:
LVRhase01 26:223339e60e00 192 led1=0;
LVRhase01 26:223339e60e00 193 led3=1;
LVRhase01 26:223339e60e00 194 led4=0;
LVRhase01 26:223339e60e00 195 arduino.getData(weight);
LVRhase01 26:223339e60e00 196 for (uint8_t i = 0; i < 4; i++) {
LVRhase01 26:223339e60e00 197 pc.printf("%d",weight[i]);
LVRhase01 26:223339e60e00 198 pc.printf("\t");
LVRhase01 24:58e3ac686b05 199 }
LVRhase01 26:223339e60e00 200
LVRhase01 26:223339e60e00 201 y = weight[0] + weight[2] - weight[1] - weight[3];
LVRhase01 26:223339e60e00 202 x = weight[0] + weight[1] - weight[2] - weight[3];
LVRhase01 26:223339e60e00 203 if(y < -50){
LVRhase01 26:223339e60e00 204 y = -0.5;
LVRhase01 26:223339e60e00 205 }else if(y > 50){
LVRhase01 26:223339e60e00 206 y = 0.5;
LVRhase01 26:223339e60e00 207 }else{
LVRhase01 26:223339e60e00 208 y = 0;
LVRhase01 26:223339e60e00 209 }
LVRhase01 26:223339e60e00 210 if(x < -50){
LVRhase01 26:223339e60e00 211 x = -0.5;
LVRhase01 26:223339e60e00 212 }else if(x > 50){
LVRhase01 26:223339e60e00 213 x = 0.5;
LVRhase01 26:223339e60e00 214 }else{
LVRhase01 26:223339e60e00 215 x = 0;
LVRhase01 26:223339e60e00 216 }
LVRhase01 26:223339e60e00 217 now_angle = jy.getZaxisAngle();
LVRhase01 26:223339e60e00 218
LVRhase01 26:223339e60e00 219 if(now_angle>180 && now_angle<360){
LVRhase01 26:223339e60e00 220 now_angle = now_angle-360;
LVRhase01 26:223339e60e00 221 }
LVRhase01 26:223339e60e00 222 angle.setProcessValue(now_angle);
LVRhase01 26:223339e60e00 223 omni.computeXY(
LVRhase01 26:223339e60e00 224 x,
LVRhase01 26:223339e60e00 225 y,
LVRhase01 26:223339e60e00 226 0,
LVRhase01 26:223339e60e00 227 0,
LVRhase01 26:223339e60e00 228 angle.compute()
LVRhase01 26:223339e60e00 229
LVRhase01 26:223339e60e00 230 );
LVRhase01 26:223339e60e00 231 for(int i=0; i<3; i++){
LVRhase01 26:223339e60e00 232 motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
LVRhase01 26:223339e60e00 233 }
LVRhase01 26:223339e60e00 234 pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
LVRhase01 26:223339e60e00 235 break;
LVRhase01 9:1ea57af922e7 236 }
LVRhase01 1:3d8552eac4fa 237 }
LVRhase01 1:3d8552eac4fa 238 }