Ryu Anzawa / Mbed OS wheel_test5

wheel_test5 ver_1

Dependencies:   QEI omni_wheel PID R1370 Controller ikarashiMDC

main.cpp@0:424b608bab8c, 2019-06-20 (annotated)

Committer:
piroro4560
Date:
Thu Jun 20 07:03:57 2019 +0000
Revision:
0:424b608bab8c
ver_1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:424b608bab8c 1 #include "mbed.h" //wheel_test5
piroro4560 0:424b608bab8c 2 #include "controller.h"
piroro4560 0:424b608bab8c 3 #include "ikarashiMDC.h"
piroro4560 0:424b608bab8c 4 #include "R1370.h"
piroro4560 0:424b608bab8c 5 #include "omni_wheel.h"
piroro4560 0:424b608bab8c 6 #include "PID.h"
piroro4560 0:424b608bab8c 7 #define PI 3.1415926535897932384626
piroro4560 0:424b608bab8c 8
piroro4560 0:424b608bab8c 9 PID pid1(5.0,0.2,0.000095,0.01);
piroro4560 0:424b608bab8c 10 OmniWheel omni(4);
piroro4560 0:424b608bab8c 11 Controller pad(PC_10, PC_11, 208);
piroro4560 0:424b608bab8c 12 R1370 R1370(PB_6,PA_10);
piroro4560 0:424b608bab8c 13 Serial pc(USBTX, USBRX, 115200);
piroro4560 0:424b608bab8c 14 Serial serial(PC_6, PC_7, 115200);
piroro4560 0:424b608bab8c 15 DigitalOut serialcontrol(D10);
piroro4560 0:424b608bab8c 16 //DigitalOut reset(PC_9);
piroro4560 0:424b608bab8c 17
piroro4560 0:424b608bab8c 18
piroro4560 0:424b608bab8c 19 ikarashiMDC motor[]=
piroro4560 0:424b608bab8c 20 {
piroro4560 0:424b608bab8c 21 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
piroro4560 0:424b608bab8c 22 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
piroro4560 0:424b608bab8c 23 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
piroro4560 0:424b608bab8c 24 ikarashiMDC(&serialcontrol,2,3,SM,&serial),
piroro4560 0:424b608bab8c 25 };
piroro4560 0:424b608bab8c 26
piroro4560 0:424b608bab8c 27
piroro4560 0:424b608bab8c 28 double sign(double a){
piroro4560 0:424b608bab8c 29 return(a>0)-(a<0);
piroro4560 0:424b608bab8c 30 }
piroro4560 0:424b608bab8c 31
piroro4560 0:424b608bab8c 32 int main()
piroro4560 0:424b608bab8c 33 {
piroro4560 0:424b608bab8c 34 // reset = 1;
piroro4560 0:424b608bab8c 35 motor[0].braking = true;
piroro4560 0:424b608bab8c 36 int b[11], b2[11], b3[11], angle_state;
piroro4560 0:424b608bab8c 37 double rad[2], dis[2], value[3], X, Y, original_angle=0, now_angle, start_angle, zero=0, spin_power;
piroro4560 0:424b608bab8c 38 double deviation = 0.08;
piroro4560 0:424b608bab8c 39 /**
piroro4560 0:424b608bab8c 40 * now_angle : 今の角度
piroro4560 0:424b608bab8c 41 * start_angle : 素の角度
piroro4560 0:424b608bab8c 42 * original_angle : 0,-90,90,180
piroro4560 0:424b608bab8c 43 * deviation : 差
piroro4560 0:424b608bab8c 44 * angle_state : 90度毎
piroro4560 0:424b608bab8c 45 * zero : 零点合わせ
piroro4560 0:424b608bab8c 46 */
piroro4560 0:424b608bab8c 47 pid1.setInputLimits(-180,180);
piroro4560 0:424b608bab8c 48 pid1.setOutputLimits(-0.3,0.3);
piroro4560 0:424b608bab8c 49 pid1.setBias(0);
piroro4560 0:424b608bab8c 50 pid1.setMode(1);
piroro4560 0:424b608bab8c 51 pid1.setSetPoint(0);////
piroro4560 0:424b608bab8c 52 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
piroro4560 0:424b608bab8c 53 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
piroro4560 0:424b608bab8c 54 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
piroro4560 0:424b608bab8c 55 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
piroro4560 0:424b608bab8c 56 while(1){
piroro4560 0:424b608bab8c 57 if(pad.receiveState()){
piroro4560 0:424b608bab8c 58 for(int i = 0; i < 13; i++){
piroro4560 0:424b608bab8c 59 b[i] = 1 - pad.getButton1(i);
piroro4560 0:424b608bab8c 60 }
piroro4560 0:424b608bab8c 61 for(int i = 0; i < 2; i++){
piroro4560 0:424b608bab8c 62 rad[i] = pad.getRadian(i);
piroro4560 0:424b608bab8c 63 dis[i] = pad.getNorm(i);
piroro4560 0:424b608bab8c 64 rad[i] = PI - rad[i];
piroro4560 0:424b608bab8c 65 }
piroro4560 0:424b608bab8c 66 X = dis[0] * cos(rad[0]);
piroro4560 0:424b608bab8c 67 Y = dis[0] * sin(rad[0]);
piroro4560 0:424b608bab8c 68 R1370.update();
piroro4560 0:424b608bab8c 69 for (int i = 0; i < 11; i++) b3[i] = b2[i] - b[i];
piroro4560 0:424b608bab8c 70 /**
piroro4560 0:424b608bab8c 71 * ここまでテンプル
piroro4560 0:424b608bab8c 72 */
piroro4560 0:424b608bab8c 73 start_angle = R1370.getAngle();
piroro4560 0:424b608bab8c 74
piroro4560 0:424b608bab8c 75 if ( b3[9] == 1) angle_state++;
piroro4560 0:424b608bab8c 76 if ( b3[7] == 1) angle_state--;
piroro4560 0:424b608bab8c 77 angle_state %= 4;
piroro4560 0:424b608bab8c 78 switch( ( angle_state + 8 ) % 4 ){
piroro4560 0:424b608bab8c 79 case 1: original_angle=180;
piroro4560 0:424b608bab8c 80 break;
piroro4560 0:424b608bab8c 81 case 2: original_angle=-90;
piroro4560 0:424b608bab8c 82 break;
piroro4560 0:424b608bab8c 83 case 3: original_angle=0;
piroro4560 0:424b608bab8c 84 break;
piroro4560 0:424b608bab8c 85 default : original_angle=90;
piroro4560 0:424b608bab8c 86 break;
piroro4560 0:424b608bab8c 87 }
piroro4560 0:424b608bab8c 88 if ( b[6] ) original_angle = start_angle;
piroro4560 0:424b608bab8c 89
piroro4560 0:424b608bab8c 90 now_angle = start_angle - original_angle;
piroro4560 0:424b608bab8c 91
piroro4560 0:424b608bab8c 92 if ( now_angle > 180 ) now_angle = now_angle - 360;
piroro4560 0:424b608bab8c 93 if ( now_angle < -180 ) now_angle = now_angle + 360;
piroro4560 0:424b608bab8c 94
piroro4560 0:424b608bab8c 95 pid1.setProcessValue(now_angle);
piroro4560 0:424b608bab8c 96 /**
piroro4560 0:424b608bab8c 97 * 定型文
piroro4560 0:424b608bab8c 98 */
piroro4560 0:424b608bab8c 99 spin_power = pid1.compute();
piroro4560 0:424b608bab8c 100 omni.computeXY(X,Y,-spin_power);
piroro4560 0:424b608bab8c 101 pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||%d\n\r",X, Y, start_angle, now_angle, spin_power, angle_state);
piroro4560 0:424b608bab8c 102 for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]);
piroro4560 0:424b608bab8c 103 for (int i = 0; i < 11; i++) b2[i] = b[i];
piroro4560 0:424b608bab8c 104 }else{
piroro4560 0:424b608bab8c 105 pc.printf("error\n\r");
piroro4560 0:424b608bab8c 106 for (int i = 0; i < 4; i++)motor[i].setSpeed(0);
piroro4560 0:424b608bab8c 107 }
piroro4560 0:424b608bab8c 108 }
piroro4560 0:424b608bab8c 109 }