NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

ハセオムニのプログラム

Dependencies:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@20:dfda3dbb9007, 2020-10-17 (annotated)

Committer:
LVRhase01
Date:
Sat Oct 17 08:09:11 2020 +0000
Revision:
20:dfda3dbb9007
Parent:
8:89827b0d8a93
Child:
21:f563d813b5c5
Child:
22:a9200d209736
wii balance board + JY901 ver hase+haseomni circuit ver2

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 4:07dc5c86e702 5 #include "motorsmlap.h"
LVRhase01 7:4e77a1ae70d1 6 #include "jy901.h"
LVRhase01 4:07dc5c86e702 7 #include "SerialMultiByte.h"
LVRhase01 7:4e77a1ae70d1 8 #include "PID.h"
LVRhase01 1:3d8552eac4fa 9
LVRhase01 7:4e77a1ae70d1 10 JY901 jy(PB_9, PB_8); //sda, scl
LVRhase01 7:4e77a1ae70d1 11
LVRhase01 7:4e77a1ae70d1 12 PID angle(2.0,0,0,0.01);
LVRhase01 1:3d8552eac4fa 13 Serial pc(USBTX, USBRX, 115200);
LVRhase01 20:dfda3dbb9007 14 SerialMultiByte arduino(PC_12,PD_2);
LVRhase01 4:07dc5c86e702 15
LVRhase01 1:3d8552eac4fa 16 PwmOut beep(PA_0);
LVRhase01 1:3d8552eac4fa 17 DigitalOut led1(PA_11);
LVRhase01 1:3d8552eac4fa 18 DigitalOut led2(LED2);
LVRhase01 1:3d8552eac4fa 19
LVRhase01 4:07dc5c86e702 20 motorSmLap motor[] = {
LVRhase01 4:07dc5c86e702 21 motorSmLap(PC_6, PC_9, PC_8),
LVRhase01 4:07dc5c86e702 22 motorSmLap(PB_6, PA_9, PC_7),
LVRhase01 4:07dc5c86e702 23 motorSmLap(PA_8, PB_4, PB_7)
LVRhase01 4:07dc5c86e702 24 };
LVRhase01 5:3eed67b60cd2 25
LVRhase01 1:3d8552eac4fa 26 const double PII = 3.14159265358979;
LVRhase01 7:4e77a1ae70d1 27 OmniWheel omni(3);
LVRhase01 0:b3c48e055e7b 28
LVRhase01 1:3d8552eac4fa 29 void setup(){
LVRhase01 7:4e77a1ae70d1 30 arduino.setHeaders(127,127);
LVRhase01 7:4e77a1ae70d1 31 arduino.startReceive(5);
LVRhase01 1:3d8552eac4fa 32 omni.wheel[0].setRadian(PII/2.0);
LVRhase01 3:9e20624d3d0d 33 omni.wheel[1].setRadian(7.0/6.0*PII);
LVRhase01 3:9e20624d3d0d 34 omni.wheel[2].setRadian(11.0/6.0*PII);
LVRhase01 1:3d8552eac4fa 35
LVRhase01 1:3d8552eac4fa 36 beep.period(1.0/3136);
LVRhase01 1:3d8552eac4fa 37 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 38 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 39
LVRhase01 1:3d8552eac4fa 40 beep.period(1.0/2960);
LVRhase01 1:3d8552eac4fa 41 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 42 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 43
LVRhase01 1:3d8552eac4fa 44 beep.period(1.0/2489);
LVRhase01 1:3d8552eac4fa 45 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 46 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 47
LVRhase01 1:3d8552eac4fa 48 beep.period(1.0/1760);
LVRhase01 1:3d8552eac4fa 49 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 50 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 51
LVRhase01 1:3d8552eac4fa 52 beep.period(1.0/1661);
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/2637);
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/3322);
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/4186);
LVRhase01 1:3d8552eac4fa 65 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 66 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 67
LVRhase01 1:3d8552eac4fa 68 beep.write(0.0f);
LVRhase01 1:3d8552eac4fa 69 wait_ms(BEAT);
LVRhase01 0:b3c48e055e7b 70 }
LVRhase01 0:b3c48e055e7b 71
LVRhase01 1:3d8552eac4fa 72 int main() {
LVRhase01 1:3d8552eac4fa 73 setup();
LVRhase01 8:89827b0d8a93 74 float x, y,now_angle;
LVRhase01 7:4e77a1ae70d1 75 jy.calibrateAll(50);
LVRhase01 7:4e77a1ae70d1 76 uint8_t weight[5];
LVRhase01 7:4e77a1ae70d1 77 angle.setInputLimits(-180.0,180.0);
LVRhase01 7:4e77a1ae70d1 78 angle.setOutputLimits(-1,1);
LVRhase01 7:4e77a1ae70d1 79 angle.setBias(0);
LVRhase01 7:4e77a1ae70d1 80 angle.setMode(1);
LVRhase01 7:4e77a1ae70d1 81 angle.setSetPoint(0);
LVRhase01 1:3d8552eac4fa 82 while (true) {
LVRhase01 5:3eed67b60cd2 83 led2=1;
LVRhase01 7:4e77a1ae70d1 84 arduino.getData(weight);
LVRhase01 7:4e77a1ae70d1 85 for (uint8_t i = 0; i < 4; i++) {
LVRhase01 7:4e77a1ae70d1 86 pc.printf("%d",weight[i]);
LVRhase01 4:07dc5c86e702 87 pc.printf("\t");
LVRhase01 4:07dc5c86e702 88 }
LVRhase01 7:4e77a1ae70d1 89
LVRhase01 7:4e77a1ae70d1 90 y = weight[0] + weight[2] - weight[1] - weight[3];
LVRhase01 7:4e77a1ae70d1 91 x = weight[0] + weight[1] - weight[2] - weight[3];
LVRhase01 7:4e77a1ae70d1 92 if(y < -50){
LVRhase01 4:07dc5c86e702 93 y = -0.5;
LVRhase01 7:4e77a1ae70d1 94 }else if(y > 50){
LVRhase01 7:4e77a1ae70d1 95 y = 0.5;
LVRhase01 7:4e77a1ae70d1 96 }else{
LVRhase01 7:4e77a1ae70d1 97 y = 0;
LVRhase01 6:6ce8adb328fa 98 }
LVRhase01 7:4e77a1ae70d1 99 if(x < -50){
LVRhase01 7:4e77a1ae70d1 100 x = -0.5;
LVRhase01 7:4e77a1ae70d1 101 }else if(x > 50){
LVRhase01 7:4e77a1ae70d1 102 x = 0.5;
LVRhase01 7:4e77a1ae70d1 103 }else{
LVRhase01 7:4e77a1ae70d1 104 x = 0;
LVRhase01 4:07dc5c86e702 105 }
LVRhase01 7:4e77a1ae70d1 106 now_angle = jy.getZaxisAngle();
LVRhase01 6:6ce8adb328fa 107
LVRhase01 7:4e77a1ae70d1 108 if(now_angle>180 && now_angle<360){
LVRhase01 7:4e77a1ae70d1 109 now_angle = now_angle-360;
LVRhase01 6:6ce8adb328fa 110 }
LVRhase01 7:4e77a1ae70d1 111 angle.setProcessValue(now_angle);
LVRhase01 1:3d8552eac4fa 112 omni.computeXY(
LVRhase01 1:3d8552eac4fa 113 x,
LVRhase01 1:3d8552eac4fa 114 y,
LVRhase01 1:3d8552eac4fa 115 0,
LVRhase01 1:3d8552eac4fa 116 0,
LVRhase01 7:4e77a1ae70d1 117 angle.compute()
LVRhase01 7:4e77a1ae70d1 118
LVRhase01 1:3d8552eac4fa 119 );
LVRhase01 2:5a55f555e885 120 for(int i=0; i<3; i++){
LVRhase01 4:07dc5c86e702 121 motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
LVRhase01 2:5a55f555e885 122 }
LVRhase01 8:89827b0d8a93 123 //pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
LVRhase01 1:3d8552eac4fa 124 }
LVRhase01 5:3eed67b60cd2 125 }