NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

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:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@27:2d7a978e70ab, 2022-02-03 (annotated)

Committer:
LVRhase01
Date:
Thu Feb 03 08:34:06 2022 +0000
Revision:
27:2d7a978e70ab
Parent:
26:223339e60e00 
first commit haseomni

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