春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_main_ver4

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:   mbed EC PathFollowing-ver10 CruizCore_R1370P

main.cpp@1:86eae1cf26d2, 2018-11-16 (annotated)

Committer:
la00noix
Date:
Fri Nov 16 23:32:58 2018 +0000
Revision:
1:86eae1cf26d2
Parent:
0:f5992b0c6e00
Child:
2:e04e6b5d6584
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:f5992b0c6e00 1 #include "mbed.h"
la00noix 0:f5992b0c6e00 2 #include "EC.h"
la00noix 0:f5992b0c6e00 3 #include "R1370P.h"
la00noix 0:f5992b0c6e00 4 #include "move4wheel.h"
la00noix 0:f5992b0c6e00 5 #include "PathFollowing.h"
la00noix 0:f5992b0c6e00 6 #include <stdarg.h>
la00noix 0:f5992b0c6e00 7
la00noix 0:f5992b0c6e00 8 #define PI 3.141592
la00noix 0:f5992b0c6e00 9
la00noix 0:f5992b0c6e00 10 #define DEBUG_MODE // compile as debug mode (comment out if you don't use)
la00noix 0:f5992b0c6e00 11 #ifdef DEBUG_MODE
la00noix 0:f5992b0c6e00 12 #define DEBUG_PRINT // enable debug_printf
la00noix 0:f5992b0c6e00 13 #endif
la00noix 0:f5992b0c6e00 14
la00noix 0:f5992b0c6e00 15 Serial pc(USBTX,USBRX);
la00noix 0:f5992b0c6e00 16 void debug_printf(const char* format,...); // work as printf in debug
la00noix 0:f5992b0c6e00 17 void Debug_Control(); // control by PC keybord
la00noix 0:f5992b0c6e00 18
la00noix 0:f5992b0c6e00 19 #define SPI_FREQ 1000000 // 1MHz
la00noix 0:f5992b0c6e00 20 #define SPI_BITS 16
la00noix 0:f5992b0c6e00 21 #define SPI_MODE 0
la00noix 0:f5992b0c6e00 22 #define SPI_WAIT_US 1 // 1us
la00noix 0:f5992b0c6e00 23 SPI spi(PB_5,PB_4,PB_3);
la00noix 0:f5992b0c6e00 24
la00noix 0:f5992b0c6e00 25 DigitalOut ss_md1(PB_15); //エスコンの設定
la00noix 0:f5992b0c6e00 26 DigitalOut ss_md2(PB_14);
la00noix 0:f5992b0c6e00 27 DigitalOut ss_md3(PB_13);
la00noix 0:f5992b0c6e00 28 DigitalOut ss_md4(PC_4);
la00noix 0:f5992b0c6e00 29
la00noix 0:f5992b0c6e00 30 DigitalOut md_enable(PA_13); // do all motor driver enable
la00noix 0:f5992b0c6e00 31 //DigitalIn md_ch_enable(p10); // check enable switch is open or close
la00noix 0:f5992b0c6e00 32 //Timer md_disable;
la00noix 0:f5992b0c6e00 33 DigitalOut md_stop(PA_14); // stop all motor
la00noix 0:f5992b0c6e00 34 DigitalIn md_check(PB_7); // check error of all motor driver //とりあえず使わない
la00noix 0:f5992b0c6e00 35
la00noix 0:f5992b0c6e00 36 /*モーターの配置
la00noix 0:f5992b0c6e00 37 * md1//---F---\\md4
la00noix 0:f5992b0c6e00 38 * | |
la00noix 0:f5992b0c6e00 39 * L + R
la00noix 0:f5992b0c6e00 40 * | |
la00noix 0:f5992b0c6e00 41 * md2\\---B---//md3
la00noix 0:f5992b0c6e00 42 */
la00noix 0:f5992b0c6e00 43
la00noix 0:f5992b0c6e00 44
la00noix 0:f5992b0c6e00 45 Ec EC1(PC_6,PC_8,NC,500,0.05);
la00noix 0:f5992b0c6e00 46 Ec EC2(PB_1,PB_12,NC,500,0.05); //エンコーダ
la00noix 0:f5992b0c6e00 47 Ticker motor_tick; //角速度計算用ticker
la00noix 0:f5992b0c6e00 48 Ticker ticker; //for enc
la00noix 0:f5992b0c6e00 49
la00noix 0:f5992b0c6e00 50 R1370P gyro(PC_6,PC_7); //ジャイロ
la00noix 0:f5992b0c6e00 51
la00noix 0:f5992b0c6e00 52 //DigitalOut can_led(LED1); //if can enable -> toggle
la00noix 0:f5992b0c6e00 53 DigitalOut debug_led(LED2); //if debugmode -> on
la00noix 0:f5992b0c6e00 54 DigitalOut md_stop_led(LED3); //if motor stop -> on
la00noix 0:f5992b0c6e00 55 DigitalOut md_err_led(LED4); //if driver error -> on //とりあえず使わない
la00noix 0:f5992b0c6e00 56
la00noix 0:f5992b0c6e00 57 double new_dist1=0,new_dist2=0;
la00noix 0:f5992b0c6e00 58 double old_dist1=0,old_dist2=0;
la00noix 0:f5992b0c6e00 59 double d_dist1=0,d_dist2=0; //座標計算用関数
la00noix 0:f5992b0c6e00 60 double d_x,d_y;
la00noix 0:f5992b0c6e00 61 //現在地X,y座標、現在角度については、PathFollowingでnow_x,now_y,now_angleを定義済
la00noix 0:f5992b0c6e00 62 double start_x=0,start_y=0; //スタート位置
la00noix 0:f5992b0c6e00 63
la00noix 0:f5992b0c6e00 64 static int16_t m1=0, m2=0, m3=0, m4=0; //int16bit = int2byte
la00noix 0:f5992b0c6e00 65
la00noix 0:f5992b0c6e00 66 void UserLoopSetting(); // initialize setting
la00noix 0:f5992b0c6e00 67 void DAC_Write(int16_t data, DigitalOut* DAC_cs);
la00noix 0:f5992b0c6e00 68 void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4);
la00noix 0:f5992b0c6e00 69
la00noix 0:f5992b0c6e00 70 void calOmega() //角速度計算関数
la00noix 0:f5992b0c6e00 71 {
la00noix 0:f5992b0c6e00 72 EC1.CalOmega();
la00noix 0:f5992b0c6e00 73 EC2.CalOmega();
la00noix 0:f5992b0c6e00 74 }
la00noix 0:f5992b0c6e00 75
la00noix 0:f5992b0c6e00 76 void output(double FL,double BL,double BR,double FR)
la00noix 0:f5992b0c6e00 77 {
la00noix 0:f5992b0c6e00 78 m1=FL;
la00noix 0:f5992b0c6e00 79 m2=BL;
la00noix 0:f5992b0c6e00 80 m3=BR;
la00noix 0:f5992b0c6e00 81 m4=FR;
la00noix 0:f5992b0c6e00 82 }
la00noix 0:f5992b0c6e00 83
la00noix 0:f5992b0c6e00 84 void base(double FL,double BL,double BR,double FR,double Max)
la00noix 0:f5992b0c6e00 85 //いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
la00noix 0:f5992b0c6e00 86 //マクソンは-4095～4095だからMax=4095にする//最速スピードを出すための関数になってる
la00noix 0:f5992b0c6e00 87 {
la00noix 0:f5992b0c6e00 88 if (fabs(FL)>=fabs(BL)&&fabs(FL)>=fabs(BR)&&fabs(FL)>=fabs(FR))output(Max ,Max*BL/fabs(FL),Max*BR/fabs(FL),Max*FR/fabs(FL));
la00noix 0:f5992b0c6e00 89 else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max ,Max*BR/fabs(BL),Max*FR/fabs(BL));
la00noix 0:f5992b0c6e00 90 else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max ,Max*FR/fabs(BR));
la00noix 0:f5992b0c6e00 91 else output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max );
la00noix 0:f5992b0c6e00 92 }
la00noix 0:f5992b0c6e00 93
la00noix 0:f5992b0c6e00 94 //ここからそれぞれのプログラム//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
la00noix 0:f5992b0c6e00 95 //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
la00noix 0:f5992b0c6e00 96 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
la00noix 0:f5992b0c6e00 97 //通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
la00noix 0:f5992b0c6e00 98 //ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね
la00noix 0:f5992b0c6e00 99
la00noix 0:f5992b0c6e00 100 void purecurve(int type,double X,double Y,double r,int theta,double speed/*,double v*/)
la00noix 0:f5992b0c6e00 101 {
la00noix 0:f5992b0c6e00 102 //正面を変えずに円弧を描いて90°曲がる
la00noix 0:f5992b0c6e00 103 //X=円弧の中心座標、Y=円弧の中心座標、r=円弧の半径、theta=plotの間隔(0～90°)、v=目標速度
la00noix 0:f5992b0c6e00 104
la00noix 0:f5992b0c6e00 105 int s;
la00noix 0:f5992b0c6e00 106 int t = 0;
la00noix 0:f5992b0c6e00 107 double plotx[(90/theta)+1]; //円弧にとるplotのx座標
la00noix 0:f5992b0c6e00 108 double ploty[(90/theta)+1];
la00noix 0:f5992b0c6e00 109 //double plotvx[(90/theta)+1]; //各plotにおける速度
la00noix 0:f5992b0c6e00 110 //double plotvy[(90/theta)+1];
la00noix 0:f5992b0c6e00 111
la00noix 0:f5992b0c6e00 112 double x_out,y_out,r_out;
la00noix 0:f5992b0c6e00 113
la00noix 0:f5992b0c6e00 114 switch(type) {
la00noix 0:f5992b0c6e00 115 case 1://↑から→
la00noix 0:f5992b0c6e00 116
la00noix 0:f5992b0c6e00 117 for(s=0; s<((90/theta)+1); s++) {
la00noix 0:f5992b0c6e00 118 plotx[s] = X + r * cos(PI - s * (PI*theta/180)) + r;
la00noix 0:f5992b0c6e00 119 ploty[s] = Y + r * sin(PI - s * (PI*theta/180));
la00noix 0:f5992b0c6e00 120 //plotvx[s] = -v * cos(PI - s * (PI*theta/180));
la00noix 0:f5992b0c6e00 121 //plotvy[s] = v * sin(PI - s * (PI*theta/180));
la00noix 0:f5992b0c6e00 122 //printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
la00noix 0:f5992b0c6e00 123 }
la00noix 0:f5992b0c6e00 124
la00noix 0:f5992b0c6e00 125 while(1) {
la00noix 0:f5992b0c6e00 126 now_angle=gyro.getAngle(); //ジャイロの値読み込み
la00noix 0:f5992b0c6e00 127
la00noix 0:f5992b0c6e00 128 new_dist1=EC1.getDistance_mm();
la00noix 0:f5992b0c6e00 129 new_dist2=EC2.getDistance_mm();
la00noix 0:f5992b0c6e00 130 d_dist1=new_dist1-old_dist1;
la00noix 0:f5992b0c6e00 131 d_dist2=new_dist2-old_dist2;
la00noix 0:f5992b0c6e00 132 old_dist1=new_dist1;
la00noix 0:f5992b0c6e00 133 old_dist2=new_dist2; //微小時間当たりのエンコーダ読み込み
la00noix 0:f5992b0c6e00 134
la00noix 0:f5992b0c6e00 135 d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180);
la00noix 0:f5992b0c6e00 136 d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180); //微小時間毎の座標変化
la00noix 0:f5992b0c6e00 137 now_x=now_x+d_x;
la00noix 0:f5992b0c6e00 138 now_y=now_y+d_y; //微小時間毎に座標に加算
la00noix 0:f5992b0c6e00 139
la00noix 0:f5992b0c6e00 140 XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
la00noix 0:f5992b0c6e00 141 CalMotorOut(x_out,y_out,r_out); //move4wheel内のモーター番号定義または成分分解が違うかも？
la00noix 0:f5992b0c6e00 142 //CalMotorOut(plotvx[t], plotvy[t],0);
la00noix 0:f5992b0c6e00 143
la00noix 0:f5992b0c6e00 144 //printf("t=%d x_out=%f y_out=%f\n\r",t,x_out,y_out);
la00noix 0:f5992b0c6e00 145 //printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
la00noix 0:f5992b0c6e00 146
la00noix 0:f5992b0c6e00 147 output(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); //m1～m4に代入
la00noix 0:f5992b0c6e00 148
la00noix 0:f5992b0c6e00 149 if(((X - now_x)*(plotx[t+1] - plotx[t]) + (Y - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
la00noix 0:f5992b0c6e00 150 if(t == (90/theta))break;
la00noix 0:f5992b0c6e00 151
la00noix 0:f5992b0c6e00 152 MotorControl(m1,m2,m3,m4); //出力
la00noix 0:f5992b0c6e00 153
la00noix 0:f5992b0c6e00 154 //printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f\n\r",m1,m2,m3,m4,now_x,now_y);
la00noix 0:f5992b0c6e00 155
la00noix 0:f5992b0c6e00 156 }
la00noix 0:f5992b0c6e00 157
la00noix 0:f5992b0c6e00 158 case 2://↑から← //まだ編集してない
la00noix 0:f5992b0c6e00 159
la00noix 0:f5992b0c6e00 160 for(s=0; s<((90/theta)+1); s++) {
la00noix 0:f5992b0c6e00 161 plotx[s] = X + r * cos(s * (PI*theta/180));
la00noix 0:f5992b0c6e00 162 ploty[s] = Y + r * sin(s * (PI*theta/180));
la00noix 0:f5992b0c6e00 163 }
la00noix 0:f5992b0c6e00 164
la00noix 0:f5992b0c6e00 165 while(1) {
la00noix 0:f5992b0c6e00 166
la00noix 0:f5992b0c6e00 167 now_angle=gyro.getAngle(); //ジャイロの値読み込み
la00noix 0:f5992b0c6e00 168
la00noix 0:f5992b0c6e00 169 new_dist1=EC1.getDistance_mm();
la00noix 0:f5992b0c6e00 170 new_dist2=EC2.getDistance_mm();
la00noix 0:f5992b0c6e00 171 d_dist1=new_dist1-old_dist1;
la00noix 0:f5992b0c6e00 172 d_dist2=new_dist2-old_dist2;
la00noix 0:f5992b0c6e00 173 old_dist1=new_dist1;
la00noix 0:f5992b0c6e00 174 old_dist2=new_dist2; //微小時間当たりのエンコーダ読み込み
la00noix 0:f5992b0c6e00 175
la00noix 0:f5992b0c6e00 176 d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180);
la00noix 0:f5992b0c6e00 177 d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180); //微小時間毎の座標変化
la00noix 0:f5992b0c6e00 178 now_x=now_x+d_x;
la00noix 0:f5992b0c6e00 179 now_y=now_y+d_y; //微小時間毎に座標に加算
la00noix 0:f5992b0c6e00 180
la00noix 0:f5992b0c6e00 181 XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
la00noix 0:f5992b0c6e00 182 CalMotorOut(x_out,y_out,r_out);
la00noix 0:f5992b0c6e00 183 base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);
la00noix 0:f5992b0c6e00 184 if(((X - now_x)*(plotx[t+1] - plotx[t]) + (Y - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
la00noix 0:f5992b0c6e00 185 if(t == (90/theta))break;
la00noix 0:f5992b0c6e00 186
la00noix 0:f5992b0c6e00 187 }
la00noix 0:f5992b0c6e00 188 }
la00noix 0:f5992b0c6e00 189 }
la00noix 0:f5992b0c6e00 190 //ここまで///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
la00noix 0:f5992b0c6e00 191
la00noix 0:f5992b0c6e00 192 int main()
la00noix 0:f5992b0c6e00 193 {
la00noix 0:f5992b0c6e00 194 UserLoopSetting();
la00noix 0:f5992b0c6e00 195
la00noix 0:f5992b0c6e00 196 /*void reset();
la00noix 0:f5992b0c6e00 197 EC1.reset();
la00noix 0:f5992b0c6e00 198 EC2.reset();*/
la00noix 0:f5992b0c6e00 199
la00noix 0:f5992b0c6e00 200 now_x=start_x;
la00noix 0:f5992b0c6e00 201 now_y=start_y;
la00noix 0:f5992b0c6e00 202
la00noix 0:f5992b0c6e00 203 //m1, m2, m3, m4 に出力を代入すればとりあえず動く
la00noix 0:f5992b0c6e00 204
la00noix 0:f5992b0c6e00 205 while(1) {
la00noix 0:f5992b0c6e00 206
la00noix 0:f5992b0c6e00 207 //Debug_Control();
la00noix 0:f5992b0c6e00 208 purecurve(1,0,0,1000,9,1000);
la00noix 0:f5992b0c6e00 209 //MotorControl(m1,m2,m3,m4);
la00noix 0:f5992b0c6e00 210
la00noix 0:f5992b0c6e00 211 }
la00noix 0:f5992b0c6e00 212 }
la00noix 0:f5992b0c6e00 213
la00noix 0:f5992b0c6e00 214 void UserLoopSetting()
la00noix 0:f5992b0c6e00 215 {
la00noix 0:f5992b0c6e00 216 //-----エスコンの初期設定-----//
la00noix 0:f5992b0c6e00 217 spi.format(SPI_BITS, SPI_MODE);
la00noix 0:f5992b0c6e00 218 spi.frequency(SPI_FREQ);
la00noix 0:f5992b0c6e00 219 ss_md1 = 1;
la00noix 0:f5992b0c6e00 220 ss_md2 = 1;
la00noix 0:f5992b0c6e00 221 ss_md3 = 1;
la00noix 0:f5992b0c6e00 222 ss_md4 = 1;
la00noix 0:f5992b0c6e00 223 md_enable = 1; //enable on
la00noix 0:f5992b0c6e00 224 md_err_led = 0;
la00noix 0:f5992b0c6e00 225 md_stop = 1;
la00noix 0:f5992b0c6e00 226 md_stop_led = 1;
la00noix 0:f5992b0c6e00 227 //-----センサーの初期設定-----//
la00noix 0:f5992b0c6e00 228 gyro.initialize();
la00noix 0:f5992b0c6e00 229 motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
la00noix 0:f5992b0c6e00 230 EC1.setDiameter_mm(48);
la00noix 0:f5992b0c6e00 231 EC2.setDiameter_mm(48); //測定輪半径
la00noix 0:f5992b0c6e00 232 //-----PathFollowingのパラメーター設定-----//
la00noix 0:f5992b0c6e00 233 set_p_out(1000); //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
la00noix 0:f5992b0c6e00 234 q_setPDparam(30,30); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
la00noix 0:f5992b0c6e00 235 r_setPDparam(30,30); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
la00noix 0:f5992b0c6e00 236 set_r_out(1000); //旋回時の最大出力値設定関数
la00noix 0:f5992b0c6e00 237 set_target_angle(0); //機体目標角度設定関数
la00noix 0:f5992b0c6e00 238
la00noix 0:f5992b0c6e00 239 #ifdef DEBUG_MODE
la00noix 0:f5992b0c6e00 240 debug_led = 1;
la00noix 0:f5992b0c6e00 241 pc.attach(Debug_Control, Serial::RxIrq);
la00noix 0:f5992b0c6e00 242 #else
la00noix 0:f5992b0c6e00 243 debug_led = 0;
la00noix 0:f5992b0c6e00 244 #endif
la00noix 0:f5992b0c6e00 245 }
la00noix 0:f5992b0c6e00 246
la00noix 0:f5992b0c6e00 247 #define MCP4922_AB (1<<15)
la00noix 0:f5992b0c6e00 248 #define MCP4922_BUF (1<<14)
la00noix 0:f5992b0c6e00 249 #define MCP4922_GA (1<<13)
la00noix 0:f5992b0c6e00 250 #define MCP4922_SHDN (1<<12)
la00noix 0:f5992b0c6e00 251
la00noix 0:f5992b0c6e00 252 #define MCP4922_SET_OUTA (0x3000) //( MCP4922_GA || MCP4922_SHDN ) //12288
la00noix 0:f5992b0c6e00 253 #define MCP4922_SET_OUTB (0xB000) //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN ) //45056
la00noix 0:f5992b0c6e00 254 #define MCP4922_MASKSET (0x0FFF) //4095
la00noix 0:f5992b0c6e00 255
la00noix 0:f5992b0c6e00 256 void DAC_Write(int16_t data, DigitalOut* DAC_cs) //(出力,出力場所)
la00noix 0:f5992b0c6e00 257 {
la00noix 0:f5992b0c6e00 258 static uint16_t dataA; //送るデータ
la00noix 0:f5992b0c6e00 259 static uint16_t dataB;
la00noix 0:f5992b0c6e00 260
la00noix 0:f5992b0c6e00 261 dataA = MCP4922_SET_OUTA;
la00noix 0:f5992b0c6e00 262 dataB = MCP4922_SET_OUTB;
la00noix 0:f5992b0c6e00 263
la00noix 0:f5992b0c6e00 264 if(data >= 0) {
la00noix 0:f5992b0c6e00 265 if(data > 4095) {
la00noix 0:f5992b0c6e00 266 data = 4095;
la00noix 0:f5992b0c6e00 267 }
la00noix 0:f5992b0c6e00 268 dataA += (MCP4922_MASKSET & (uint16_t)(data));
la00noix 0:f5992b0c6e00 269 } else {
la00noix 0:f5992b0c6e00 270 if(data < -4095) {
la00noix 0:f5992b0c6e00 271 data = -4095;
la00noix 0:f5992b0c6e00 272 }
la00noix 0:f5992b0c6e00 273 dataB += (MCP4922_MASKSET & (uint16_t)(-data));
la00noix 0:f5992b0c6e00 274 }
la00noix 0:f5992b0c6e00 275
la00noix 0:f5992b0c6e00 276 //Aの出力設定
la00noix 0:f5992b0c6e00 277 (DigitalOut)(*DAC_cs)=0;
la00noix 0:f5992b0c6e00 278 wait_us(SPI_WAIT_US);
la00noix 0:f5992b0c6e00 279 spi.write(dataA);
la00noix 0:f5992b0c6e00 280 wait_us(SPI_WAIT_US);
la00noix 0:f5992b0c6e00 281 (DigitalOut)(*DAC_cs)=1;
la00noix 0:f5992b0c6e00 282 wait_us(SPI_WAIT_US);
la00noix 0:f5992b0c6e00 283
la00noix 0:f5992b0c6e00 284 //Bの出力設定
la00noix 0:f5992b0c6e00 285 (DigitalOut)(*DAC_cs)=0;
la00noix 0:f5992b0c6e00 286 wait_us(SPI_WAIT_US);
la00noix 0:f5992b0c6e00 287 spi.write(dataB);
la00noix 0:f5992b0c6e00 288 wait_us(SPI_WAIT_US);
la00noix 0:f5992b0c6e00 289 (DigitalOut)(*DAC_cs)=1;
la00noix 0:f5992b0c6e00 290
la00noix 0:f5992b0c6e00 291 }
la00noix 0:f5992b0c6e00 292
la00noix 0:f5992b0c6e00 293 void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力
la00noix 0:f5992b0c6e00 294 {
la00noix 0:f5992b0c6e00 295 static int16_t zero_check;
la00noix 0:f5992b0c6e00 296
la00noix 0:f5992b0c6e00 297 DAC_Write(val_md1, &ss_md1);
la00noix 0:f5992b0c6e00 298 DAC_Write(val_md2, &ss_md2);
la00noix 0:f5992b0c6e00 299 DAC_Write(val_md3, &ss_md3);
la00noix 0:f5992b0c6e00 300 DAC_Write(val_md4, &ss_md4);
la00noix 0:f5992b0c6e00 301
la00noix 0:f5992b0c6e00 302 zero_check = (val_md1 | val_md2 | val_md3 | val_md4); //すべての出力が0なら強制停止
la00noix 0:f5992b0c6e00 303 if(zero_check == 0) {
la00noix 0:f5992b0c6e00 304 md_stop = 1;
la00noix 0:f5992b0c6e00 305 md_stop_led = 1;
la00noix 0:f5992b0c6e00 306 } else {
la00noix 0:f5992b0c6e00 307 md_stop = 0;
la00noix 0:f5992b0c6e00 308 md_stop_led = 0;
la00noix 0:f5992b0c6e00 309 }
la00noix 0:f5992b0c6e00 310 }
la00noix 0:f5992b0c6e00 311
la00noix 0:f5992b0c6e00 312 #ifdef DEBUG_MODE
la00noix 0:f5992b0c6e00 313 void Debug_Control()
la00noix 0:f5992b0c6e00 314 {
la00noix 0:f5992b0c6e00 315 static char pc_command = '\0';
la00noix 0:f5992b0c6e00 316
la00noix 0:f5992b0c6e00 317 pc_command = pc.getc();
la00noix 0:f5992b0c6e00 318
la00noix 1:86eae1cf26d2 319 if(pc_command == 'w') { //前進
la00noix 0:f5992b0c6e00 320 m1+=500;
la00noix 0:f5992b0c6e00 321 m2+=500;
la00noix 0:f5992b0c6e00 322 m3-=500;
la00noix 0:f5992b0c6e00 323 m4-=500;
la00noix 1:86eae1cf26d2 324 } else if(pc_command == 's') { //後進
la00noix 0:f5992b0c6e00 325 m1-=500;
la00noix 0:f5992b0c6e00 326 m2-=500;
la00noix 0:f5992b0c6e00 327 m3+=500;
la00noix 0:f5992b0c6e00 328 m4+=500;
la00noix 1:86eae1cf26d2 329 } else if(pc_command == 'd') { //右回り
la00noix 0:f5992b0c6e00 330 m1+=500;
la00noix 0:f5992b0c6e00 331 m2+=500;
la00noix 0:f5992b0c6e00 332 m3+=500;
la00noix 0:f5992b0c6e00 333 m4+=500;
la00noix 1:86eae1cf26d2 334 } else if(pc_command == 'a') { //左回り
la00noix 0:f5992b0c6e00 335 m1-=500;
la00noix 0:f5992b0c6e00 336 m2-=500;
la00noix 0:f5992b0c6e00 337 m3-=500;
la00noix 0:f5992b0c6e00 338 m4-=500;
la00noix 0:f5992b0c6e00 339 } else {
la00noix 0:f5992b0c6e00 340 m1=0;
la00noix 0:f5992b0c6e00 341 m2=0;
la00noix 0:f5992b0c6e00 342 m3=0;
la00noix 0:f5992b0c6e00 343 m4=0;
la00noix 0:f5992b0c6e00 344 }
la00noix 0:f5992b0c6e00 345
la00noix 0:f5992b0c6e00 346 if(m1>4095) { //最大値を超えないように
la00noix 0:f5992b0c6e00 347 m1=4095;
la00noix 0:f5992b0c6e00 348 } else if(m1<-4095) {
la00noix 0:f5992b0c6e00 349 m1=-4095;
la00noix 0:f5992b0c6e00 350 }
la00noix 0:f5992b0c6e00 351 if(m2>4095) {
la00noix 0:f5992b0c6e00 352 m2=4095;
la00noix 0:f5992b0c6e00 353 } else if(m2<-4095) {
la00noix 0:f5992b0c6e00 354 m2=-4095;
la00noix 0:f5992b0c6e00 355 }
la00noix 0:f5992b0c6e00 356 if(m3>4095) {
la00noix 0:f5992b0c6e00 357 m3=4095;
la00noix 0:f5992b0c6e00 358 } else if(m3<-4095) {
la00noix 0:f5992b0c6e00 359 m3=-4095;
la00noix 0:f5992b0c6e00 360 }
la00noix 0:f5992b0c6e00 361 if(m4>4095) {
la00noix 0:f5992b0c6e00 362 m4=4095;
la00noix 0:f5992b0c6e00 363 } else if(m4<-4095) {
la00noix 0:f5992b0c6e00 364 m4=-4095;
la00noix 0:f5992b0c6e00 365 }
la00noix 0:f5992b0c6e00 366
la00noix 0:f5992b0c6e00 367 debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4);
la00noix 0:f5992b0c6e00 368 MotorControl(m1,m2,m3,m4);
la00noix 0:f5992b0c6e00 369 pc_command = '\0';
la00noix 0:f5992b0c6e00 370 }
la00noix 0:f5992b0c6e00 371 #endif
la00noix 0:f5992b0c6e00 372
la00noix 0:f5992b0c6e00 373 #ifdef DEBUG_PRINT
la00noix 0:f5992b0c6e00 374 void debug_printf(const char* format,...)
la00noix 0:f5992b0c6e00 375 {
la00noix 0:f5992b0c6e00 376 va_list arg;
la00noix 0:f5992b0c6e00 377 va_start(arg, format);
la00noix 0:f5992b0c6e00 378 vprintf(format, arg);
la00noix 0:f5992b0c6e00 379 va_end(arg);
la00noix 0:f5992b0c6e00 380 }
la00noix 0:f5992b0c6e00 381 #endif