lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

limit.h@1:7b5469bf5994, 2020-07-27 (annotated)

Committer:
gohgwaja
Date:
Mon Jul 27 23:29:30 2020 +0000
Revision:
1:7b5469bf5994
Parent:
0:7cff999a7f5c
Child:
3:7b195612e26d
2222

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gohgwaja 0:7cff999a7f5c 1 #ifndef _LIMIT_H_
gohgwaja 0:7cff999a7f5c 2 #define _LIMIT_H_
gohgwaja 0:7cff999a7f5c 3
gohgwaja 0:7cff999a7f5c 4 #define limit_debug_print false
gohgwaja 0:7cff999a7f5c 5 #define start_duty 200
gohgwaja 0:7cff999a7f5c 6
gohgwaja 0:7cff999a7f5c 7 bool limit_find[6]={false,false,false,false,false,false};
gohgwaja 0:7cff999a7f5c 8
gohgwaja 0:7cff999a7f5c 9 DigitalIn limit_sw1(LIMIT_SW1);
gohgwaja 0:7cff999a7f5c 10 DigitalIn limit_sw2(LIMIT_SW2);
gohgwaja 0:7cff999a7f5c 11 DigitalIn limit_sw3(LIMIT_SW3);
gohgwaja 0:7cff999a7f5c 12 DigitalIn limit_sw4(LIMIT_SW4);
gohgwaja 0:7cff999a7f5c 13 DigitalIn limit_sw5(LIMIT_SW5);
gohgwaja 0:7cff999a7f5c 14 DigitalIn limit_sw6(LIMIT_SW6);
gohgwaja 0:7cff999a7f5c 15
gohgwaja 0:7cff999a7f5c 16 #define limit_time 10000
gohgwaja 0:7cff999a7f5c 17
gohgwaja 0:7cff999a7f5c 18 void limit_init()
gohgwaja 0:7cff999a7f5c 19 {
gohgwaja 0:7cff999a7f5c 20 limit_sw1.mode(PullUp);
gohgwaja 0:7cff999a7f5c 21 limit_sw2.mode(PullUp);
gohgwaja 0:7cff999a7f5c 22 limit_sw3.mode(PullUp);
gohgwaja 0:7cff999a7f5c 23 limit_sw4.mode(PullUp);
gohgwaja 0:7cff999a7f5c 24 limit_sw5.mode(PullUp);
gohgwaja 0:7cff999a7f5c 25 limit_sw6.mode(PullUp);
gohgwaja 0:7cff999a7f5c 26 }
gohgwaja 0:7cff999a7f5c 27
gohgwaja 0:7cff999a7f5c 28
gohgwaja 0:7cff999a7f5c 29 double limit_ex_encoder_data[6]={0,};
gohgwaja 0:7cff999a7f5c 30 double diff_encoder_data[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 31 double sum_error[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 32 double motor_duty[6]={-60,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 33 double limit_taget_speed[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 34 double limit_result[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 35 double limit_output[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 36 double limit_error[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 37
gohgwaja 0:7cff999a7f5c 38 double axis123_taget_speed=30;
gohgwaja 0:7cff999a7f5c 39 double axis4_taget_speed=-56;
gohgwaja 0:7cff999a7f5c 40 double axis56_taget_speed=30;
gohgwaja 0:7cff999a7f5c 41
gohgwaja 0:7cff999a7f5c 42 int ccnt=0;
gohgwaja 1:7b5469bf5994 43
gohgwaja 1:7b5469bf5994 44 int limit_count1=0;
gohgwaja 1:7b5469bf5994 45 int limit_count2=0;
gohgwaja 1:7b5469bf5994 46 int limit_count3=0;
gohgwaja 1:7b5469bf5994 47 int limit_count4=0;
gohgwaja 1:7b5469bf5994 48 int limit_count5=0;
gohgwaja 1:7b5469bf5994 49 int limit_count6=0;
gohgwaja 1:7b5469bf5994 50
gohgwaja 1:7b5469bf5994 51 int limit_result1=1;
gohgwaja 1:7b5469bf5994 52 int limit_result2=1;
gohgwaja 1:7b5469bf5994 53 int limit_result3=1;
gohgwaja 1:7b5469bf5994 54 int limit_result4=1;
gohgwaja 1:7b5469bf5994 55 int limit_result5=1;
gohgwaja 1:7b5469bf5994 56 int limit_result6=1;
gohgwaja 1:7b5469bf5994 57
gohgwaja 1:7b5469bf5994 58 void limit_1()
gohgwaja 1:7b5469bf5994 59 {
gohgwaja 1:7b5469bf5994 60 wait_us(30);
gohgwaja 1:7b5469bf5994 61 if(limit_sw1==0)
gohgwaja 1:7b5469bf5994 62 limit_count1++;
gohgwaja 1:7b5469bf5994 63 else
gohgwaja 1:7b5469bf5994 64 limit_count1--;
gohgwaja 1:7b5469bf5994 65
gohgwaja 1:7b5469bf5994 66 if(limit_count1>50)
gohgwaja 1:7b5469bf5994 67 limit_count1=50;
gohgwaja 1:7b5469bf5994 68 if(limit_count1<-50)
gohgwaja 1:7b5469bf5994 69 limit_count1=-50;
gohgwaja 1:7b5469bf5994 70
gohgwaja 1:7b5469bf5994 71 if(limit_count1>30)
gohgwaja 1:7b5469bf5994 72 limit_result1=0;
gohgwaja 1:7b5469bf5994 73 if(limit_count1<-30)
gohgwaja 1:7b5469bf5994 74 limit_count1=1;
gohgwaja 1:7b5469bf5994 75 }
gohgwaja 1:7b5469bf5994 76
gohgwaja 1:7b5469bf5994 77 void limit_2()
gohgwaja 1:7b5469bf5994 78 {
gohgwaja 1:7b5469bf5994 79 wait_us(30);
gohgwaja 1:7b5469bf5994 80 if(limit_sw2==0)
gohgwaja 1:7b5469bf5994 81 limit_count2++;
gohgwaja 1:7b5469bf5994 82 else
gohgwaja 1:7b5469bf5994 83 limit_count2--;
gohgwaja 1:7b5469bf5994 84
gohgwaja 1:7b5469bf5994 85 if(limit_count2>50)
gohgwaja 1:7b5469bf5994 86 limit_count2=50;
gohgwaja 1:7b5469bf5994 87 if(limit_count2<-50)
gohgwaja 1:7b5469bf5994 88 limit_count2=-50;
gohgwaja 1:7b5469bf5994 89
gohgwaja 1:7b5469bf5994 90 if(limit_count2>30)
gohgwaja 1:7b5469bf5994 91 limit_result2=0;
gohgwaja 1:7b5469bf5994 92 if(limit_count2<-30)
gohgwaja 1:7b5469bf5994 93 limit_count2=1;
gohgwaja 1:7b5469bf5994 94 }
gohgwaja 1:7b5469bf5994 95
gohgwaja 1:7b5469bf5994 96 void limit_3()
gohgwaja 1:7b5469bf5994 97 {
gohgwaja 1:7b5469bf5994 98 wait_us(30);
gohgwaja 1:7b5469bf5994 99 if(limit_sw3==0)
gohgwaja 1:7b5469bf5994 100 limit_count3++;
gohgwaja 1:7b5469bf5994 101 else
gohgwaja 1:7b5469bf5994 102 limit_count3--;
gohgwaja 1:7b5469bf5994 103
gohgwaja 1:7b5469bf5994 104 if(limit_count3>50)
gohgwaja 1:7b5469bf5994 105 limit_count3=50;
gohgwaja 1:7b5469bf5994 106 if(limit_count3<-50)
gohgwaja 1:7b5469bf5994 107 limit_count3=-50;
gohgwaja 1:7b5469bf5994 108
gohgwaja 1:7b5469bf5994 109 if(limit_count3>30)
gohgwaja 1:7b5469bf5994 110 limit_result3=0;
gohgwaja 1:7b5469bf5994 111 if(limit_count3<-30)
gohgwaja 1:7b5469bf5994 112 limit_count3=1;
gohgwaja 1:7b5469bf5994 113 }
gohgwaja 1:7b5469bf5994 114
gohgwaja 1:7b5469bf5994 115
gohgwaja 1:7b5469bf5994 116 void limit_4()
gohgwaja 1:7b5469bf5994 117 {
gohgwaja 1:7b5469bf5994 118 wait_us(30);
gohgwaja 1:7b5469bf5994 119 if(limit_sw4==0)
gohgwaja 1:7b5469bf5994 120 limit_count4++;
gohgwaja 1:7b5469bf5994 121 else
gohgwaja 1:7b5469bf5994 122 limit_count4--;
gohgwaja 1:7b5469bf5994 123
gohgwaja 1:7b5469bf5994 124 if(limit_count4>50)
gohgwaja 1:7b5469bf5994 125 limit_count4=50;
gohgwaja 1:7b5469bf5994 126 if(limit_count4<-50)
gohgwaja 1:7b5469bf5994 127 limit_count4=-50;
gohgwaja 1:7b5469bf5994 128
gohgwaja 1:7b5469bf5994 129 if(limit_count4>30)
gohgwaja 1:7b5469bf5994 130 limit_result4=0;
gohgwaja 1:7b5469bf5994 131 if(limit_count4<-30)
gohgwaja 1:7b5469bf5994 132 limit_count4=1;
gohgwaja 1:7b5469bf5994 133 }
gohgwaja 1:7b5469bf5994 134
gohgwaja 1:7b5469bf5994 135
gohgwaja 1:7b5469bf5994 136 void limit_5()
gohgwaja 1:7b5469bf5994 137 {
gohgwaja 1:7b5469bf5994 138 wait_us(30);
gohgwaja 1:7b5469bf5994 139 if(limit_sw5==0)
gohgwaja 1:7b5469bf5994 140 limit_count5++;
gohgwaja 1:7b5469bf5994 141 else
gohgwaja 1:7b5469bf5994 142 limit_count5--;
gohgwaja 1:7b5469bf5994 143
gohgwaja 1:7b5469bf5994 144 if(limit_count5>50)
gohgwaja 1:7b5469bf5994 145 limit_count5=50;
gohgwaja 1:7b5469bf5994 146 if(limit_count5<-50)
gohgwaja 1:7b5469bf5994 147 limit_count5=-50;
gohgwaja 1:7b5469bf5994 148
gohgwaja 1:7b5469bf5994 149 if(limit_count5>30)
gohgwaja 1:7b5469bf5994 150 limit_result5=0;
gohgwaja 1:7b5469bf5994 151 if(limit_count5<-30)
gohgwaja 1:7b5469bf5994 152 limit_count5=1;
gohgwaja 1:7b5469bf5994 153 }
gohgwaja 1:7b5469bf5994 154
gohgwaja 1:7b5469bf5994 155
gohgwaja 1:7b5469bf5994 156 void limit_6()
gohgwaja 1:7b5469bf5994 157 {
gohgwaja 1:7b5469bf5994 158 wait_us(30);
gohgwaja 1:7b5469bf5994 159 if(limit_sw6==0)
gohgwaja 1:7b5469bf5994 160 limit_count6++;
gohgwaja 1:7b5469bf5994 161 else
gohgwaja 1:7b5469bf5994 162 limit_count6--;
gohgwaja 1:7b5469bf5994 163
gohgwaja 1:7b5469bf5994 164 if(limit_count6>50)
gohgwaja 1:7b5469bf5994 165 limit_count6=50;
gohgwaja 1:7b5469bf5994 166 if(limit_count6<-50)
gohgwaja 1:7b5469bf5994 167 limit_count6=-50;
gohgwaja 1:7b5469bf5994 168
gohgwaja 1:7b5469bf5994 169 if(limit_count6>30)
gohgwaja 1:7b5469bf5994 170 limit_result6=0;
gohgwaja 1:7b5469bf5994 171 if(limit_count6<-30)
gohgwaja 1:7b5469bf5994 172 limit_count6=1;
gohgwaja 1:7b5469bf5994 173 }
gohgwaja 1:7b5469bf5994 174
gohgwaja 1:7b5469bf5994 175
gohgwaja 1:7b5469bf5994 176
gohgwaja 0:7cff999a7f5c 177 bool limit_check()
gohgwaja 0:7cff999a7f5c 178 {
gohgwaja 1:7b5469bf5994 179 limit_1();
gohgwaja 1:7b5469bf5994 180 limit_2();
gohgwaja 1:7b5469bf5994 181 limit_3();
gohgwaja 1:7b5469bf5994 182 limit_4();
gohgwaja 1:7b5469bf5994 183 limit_5();
gohgwaja 1:7b5469bf5994 184 limit_6();
gohgwaja 0:7cff999a7f5c 185
gohgwaja 1:7b5469bf5994 186 int sw1 = limit_result1;
gohgwaja 1:7b5469bf5994 187 int sw2 = limit_result2;
gohgwaja 1:7b5469bf5994 188 int sw3 = limit_result3;
gohgwaja 1:7b5469bf5994 189 int sw4 = limit_result4;
gohgwaja 1:7b5469bf5994 190 int sw5 = limit_result5;
gohgwaja 1:7b5469bf5994 191 int sw6 = limit_result6;
gohgwaja 0:7cff999a7f5c 192
gohgwaja 0:7cff999a7f5c 193 encoder_read_raw();
gohgwaja 0:7cff999a7f5c 194
gohgwaja 0:7cff999a7f5c 195
gohgwaja 0:7cff999a7f5c 196 if(limit_find[0]==false)
gohgwaja 0:7cff999a7f5c 197 {
gohgwaja 0:7cff999a7f5c 198 if(limit_taget_speed[0]<axis123_taget_speed)
gohgwaja 0:7cff999a7f5c 199 limit_taget_speed[0]+=0.3;
gohgwaja 0:7cff999a7f5c 200
gohgwaja 0:7cff999a7f5c 201 motor_power(0,limit_output[0]);
gohgwaja 0:7cff999a7f5c 202 }else
gohgwaja 0:7cff999a7f5c 203 {
gohgwaja 0:7cff999a7f5c 204 if(limit_taget_speed[0]>0)
gohgwaja 0:7cff999a7f5c 205 limit_taget_speed[0]-=1;
gohgwaja 0:7cff999a7f5c 206
gohgwaja 0:7cff999a7f5c 207 if(limit_taget_speed[0]<0)
gohgwaja 0:7cff999a7f5c 208 {
gohgwaja 0:7cff999a7f5c 209 limit_taget_speed[0]=0;
gohgwaja 0:7cff999a7f5c 210 }
gohgwaja 0:7cff999a7f5c 211 motor_power(0,limit_output[0]);
gohgwaja 0:7cff999a7f5c 212 }
gohgwaja 0:7cff999a7f5c 213
gohgwaja 0:7cff999a7f5c 214 if(limit_find[1]==false)
gohgwaja 0:7cff999a7f5c 215 {
gohgwaja 0:7cff999a7f5c 216 if(limit_taget_speed[1]<axis123_taget_speed)
gohgwaja 0:7cff999a7f5c 217 limit_taget_speed[1]+=0.3;
gohgwaja 0:7cff999a7f5c 218
gohgwaja 0:7cff999a7f5c 219 motor_power(1,limit_output[1]);
gohgwaja 0:7cff999a7f5c 220 }else
gohgwaja 0:7cff999a7f5c 221 {
gohgwaja 0:7cff999a7f5c 222 if(limit_taget_speed[1]>0)
gohgwaja 0:7cff999a7f5c 223 limit_taget_speed[1]-=1;
gohgwaja 0:7cff999a7f5c 224
gohgwaja 0:7cff999a7f5c 225 if(limit_taget_speed[1]<0)
gohgwaja 0:7cff999a7f5c 226 {
gohgwaja 0:7cff999a7f5c 227 limit_taget_speed[1]=0;
gohgwaja 0:7cff999a7f5c 228 }
gohgwaja 0:7cff999a7f5c 229 motor_power(1,limit_output[1]);
gohgwaja 0:7cff999a7f5c 230 }
gohgwaja 0:7cff999a7f5c 231
gohgwaja 0:7cff999a7f5c 232 if(limit_find[2]==false)
gohgwaja 0:7cff999a7f5c 233 {
gohgwaja 0:7cff999a7f5c 234 if(limit_taget_speed[2]<axis123_taget_speed)
gohgwaja 0:7cff999a7f5c 235 limit_taget_speed[2]+=0.3;
gohgwaja 0:7cff999a7f5c 236
gohgwaja 0:7cff999a7f5c 237 motor_power(2,limit_output[2]);
gohgwaja 0:7cff999a7f5c 238 }else
gohgwaja 0:7cff999a7f5c 239 {
gohgwaja 0:7cff999a7f5c 240 if(limit_taget_speed[2]>0)
gohgwaja 0:7cff999a7f5c 241 limit_taget_speed[2]-=1;
gohgwaja 0:7cff999a7f5c 242
gohgwaja 0:7cff999a7f5c 243 if(limit_taget_speed[2]<0)
gohgwaja 0:7cff999a7f5c 244 {
gohgwaja 0:7cff999a7f5c 245 limit_taget_speed[2]=0;
gohgwaja 0:7cff999a7f5c 246 }
gohgwaja 0:7cff999a7f5c 247 motor_power(2,limit_output[2]);
gohgwaja 0:7cff999a7f5c 248 }
gohgwaja 0:7cff999a7f5c 249
gohgwaja 0:7cff999a7f5c 250 if(limit_find[3]==false)
gohgwaja 0:7cff999a7f5c 251 {
gohgwaja 0:7cff999a7f5c 252 if(limit_taget_speed[3]>axis4_taget_speed)
gohgwaja 0:7cff999a7f5c 253 limit_taget_speed[3]-=0.3;
gohgwaja 0:7cff999a7f5c 254
gohgwaja 0:7cff999a7f5c 255 motor_power(3,limit_output[3]);
gohgwaja 0:7cff999a7f5c 256 }else
gohgwaja 0:7cff999a7f5c 257 {
gohgwaja 0:7cff999a7f5c 258 if(limit_taget_speed[3]<0)
gohgwaja 0:7cff999a7f5c 259 limit_taget_speed[3]+=1;
gohgwaja 0:7cff999a7f5c 260
gohgwaja 0:7cff999a7f5c 261 if(limit_taget_speed[3]>0)
gohgwaja 0:7cff999a7f5c 262 {
gohgwaja 0:7cff999a7f5c 263 limit_taget_speed[3]=0;
gohgwaja 0:7cff999a7f5c 264 }
gohgwaja 0:7cff999a7f5c 265 motor_power(3,limit_output[3]);
gohgwaja 0:7cff999a7f5c 266 }
gohgwaja 0:7cff999a7f5c 267
gohgwaja 0:7cff999a7f5c 268 if(ccnt<450)
gohgwaja 0:7cff999a7f5c 269 {
gohgwaja 0:7cff999a7f5c 270 ccnt++;
gohgwaja 0:7cff999a7f5c 271 limit_taget_speed[0]=0;
gohgwaja 0:7cff999a7f5c 272 limit_taget_speed[1]=0;
gohgwaja 0:7cff999a7f5c 273 limit_taget_speed[2]=0;
gohgwaja 0:7cff999a7f5c 274 limit_taget_speed[3]=0;
gohgwaja 0:7cff999a7f5c 275 limit_taget_speed[4]-=0.4;
gohgwaja 0:7cff999a7f5c 276 limit_taget_speed[5]-=0.4;
gohgwaja 0:7cff999a7f5c 277 }
gohgwaja 0:7cff999a7f5c 278
gohgwaja 0:7cff999a7f5c 279 if(limit_find[4]==false)
gohgwaja 0:7cff999a7f5c 280 {
gohgwaja 0:7cff999a7f5c 281 if(limit_taget_speed[4]<axis56_taget_speed)
gohgwaja 0:7cff999a7f5c 282 limit_taget_speed[4]+=0.3;
gohgwaja 0:7cff999a7f5c 283
gohgwaja 0:7cff999a7f5c 284 motor_power(4,-limit_output[4]);
gohgwaja 0:7cff999a7f5c 285 }else
gohgwaja 0:7cff999a7f5c 286 {
gohgwaja 0:7cff999a7f5c 287 if(limit_taget_speed[4]>0)
gohgwaja 0:7cff999a7f5c 288 limit_taget_speed[4]-=1;
gohgwaja 0:7cff999a7f5c 289
gohgwaja 0:7cff999a7f5c 290 if(limit_taget_speed[4]<0)
gohgwaja 0:7cff999a7f5c 291 {
gohgwaja 0:7cff999a7f5c 292 limit_taget_speed[4]=0;
gohgwaja 0:7cff999a7f5c 293 }
gohgwaja 0:7cff999a7f5c 294 motor_power(4,-limit_output[4]);
gohgwaja 0:7cff999a7f5c 295 }
gohgwaja 0:7cff999a7f5c 296 if(limit_find[5]==false)
gohgwaja 0:7cff999a7f5c 297 {
gohgwaja 0:7cff999a7f5c 298 if(limit_taget_speed[5]<axis56_taget_speed)
gohgwaja 0:7cff999a7f5c 299 limit_taget_speed[5]+=0.3;
gohgwaja 0:7cff999a7f5c 300
gohgwaja 0:7cff999a7f5c 301 motor_power(5,-limit_output[5]);
gohgwaja 0:7cff999a7f5c 302 }else
gohgwaja 0:7cff999a7f5c 303 {
gohgwaja 0:7cff999a7f5c 304 if(limit_taget_speed[5]>0)
gohgwaja 0:7cff999a7f5c 305 limit_taget_speed[5]-=1;
gohgwaja 0:7cff999a7f5c 306
gohgwaja 0:7cff999a7f5c 307 if(limit_taget_speed[5]<0)
gohgwaja 0:7cff999a7f5c 308 {
gohgwaja 0:7cff999a7f5c 309 limit_taget_speed[5]=0;
gohgwaja 0:7cff999a7f5c 310 }
gohgwaja 0:7cff999a7f5c 311 motor_power(5,-limit_output[5]);
gohgwaja 0:7cff999a7f5c 312 }
gohgwaja 0:7cff999a7f5c 313
gohgwaja 0:7cff999a7f5c 314
gohgwaja 0:7cff999a7f5c 315 for(int i=0;i<3;i++)
gohgwaja 0:7cff999a7f5c 316 {
gohgwaja 0:7cff999a7f5c 317
gohgwaja 0:7cff999a7f5c 318 diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
gohgwaja 0:7cff999a7f5c 319 limit_ex_encoder_data[i]=encoder_data[i];
gohgwaja 0:7cff999a7f5c 320 limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]);
gohgwaja 0:7cff999a7f5c 321 sum_error[i]+=limit_error[i]*0.2;
gohgwaja 0:7cff999a7f5c 322 limit_result[i]=limit_error[i]*2+sum_error[i]*1;
gohgwaja 0:7cff999a7f5c 323
gohgwaja 0:7cff999a7f5c 324 if(limit_result[i]>200)
gohgwaja 0:7cff999a7f5c 325 limit_result[i]=200;
gohgwaja 0:7cff999a7f5c 326 if(limit_result[i]<-200)
gohgwaja 0:7cff999a7f5c 327 limit_result[i]=-200;
gohgwaja 0:7cff999a7f5c 328
gohgwaja 0:7cff999a7f5c 329 limit_output[i]=motor_duty[i]+limit_result[i];
gohgwaja 0:7cff999a7f5c 330
gohgwaja 0:7cff999a7f5c 331 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 332 pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
gohgwaja 0:7cff999a7f5c 333 }
gohgwaja 0:7cff999a7f5c 334
gohgwaja 0:7cff999a7f5c 335
gohgwaja 0:7cff999a7f5c 336 for(int i=3;i<4;i++)
gohgwaja 0:7cff999a7f5c 337 {
gohgwaja 0:7cff999a7f5c 338
gohgwaja 0:7cff999a7f5c 339 diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
gohgwaja 0:7cff999a7f5c 340 limit_ex_encoder_data[i]=encoder_data[i];
gohgwaja 0:7cff999a7f5c 341 limit_error[i]=-(diff_encoder_data[i]-limit_taget_speed[i]);
gohgwaja 0:7cff999a7f5c 342 sum_error[i]+=limit_error[i]*0.2;
gohgwaja 0:7cff999a7f5c 343 limit_result[i]=limit_error[i]*2+sum_error[i]*1;
gohgwaja 0:7cff999a7f5c 344
gohgwaja 0:7cff999a7f5c 345 if(limit_result[i]>200)
gohgwaja 0:7cff999a7f5c 346 limit_result[i]=200;
gohgwaja 0:7cff999a7f5c 347 if(limit_result[i]<-200)
gohgwaja 0:7cff999a7f5c 348 limit_result[i]=-200;
gohgwaja 0:7cff999a7f5c 349
gohgwaja 0:7cff999a7f5c 350 limit_output[i]=motor_duty[i]+limit_result[i];
gohgwaja 0:7cff999a7f5c 351
gohgwaja 0:7cff999a7f5c 352 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 353 pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
gohgwaja 0:7cff999a7f5c 354 }
gohgwaja 0:7cff999a7f5c 355
gohgwaja 0:7cff999a7f5c 356
gohgwaja 0:7cff999a7f5c 357 for(int i=4;i<6;i++)
gohgwaja 0:7cff999a7f5c 358 {
gohgwaja 0:7cff999a7f5c 359
gohgwaja 0:7cff999a7f5c 360 diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
gohgwaja 0:7cff999a7f5c 361 limit_ex_encoder_data[i]=encoder_data[i];
gohgwaja 0:7cff999a7f5c 362 limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]);
gohgwaja 0:7cff999a7f5c 363 sum_error[i]+=limit_error[i]*0.2;
gohgwaja 0:7cff999a7f5c 364 limit_result[i]=limit_error[i]*2+sum_error[i]*1;
gohgwaja 0:7cff999a7f5c 365
gohgwaja 0:7cff999a7f5c 366 if(limit_result[i]>200)
gohgwaja 0:7cff999a7f5c 367 limit_result[i]=200;
gohgwaja 0:7cff999a7f5c 368 if(limit_result[i]<-200)
gohgwaja 0:7cff999a7f5c 369 limit_result[i]=-200;
gohgwaja 0:7cff999a7f5c 370
gohgwaja 0:7cff999a7f5c 371 limit_output[i]=motor_duty[i]+limit_result[i];
gohgwaja 0:7cff999a7f5c 372
gohgwaja 0:7cff999a7f5c 373 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 374 pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
gohgwaja 0:7cff999a7f5c 375 }
gohgwaja 0:7cff999a7f5c 376
gohgwaja 0:7cff999a7f5c 377
gohgwaja 0:7cff999a7f5c 378 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 379 pc.printf("\n\r");
gohgwaja 0:7cff999a7f5c 380
gohgwaja 0:7cff999a7f5c 381
gohgwaja 0:7cff999a7f5c 382
gohgwaja 0:7cff999a7f5c 383 if(sw1==0 && !limit_find[0])
gohgwaja 0:7cff999a7f5c 384 {
gohgwaja 0:7cff999a7f5c 385 limit_find[0]=true;
gohgwaja 0:7cff999a7f5c 386 encoder1.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 387 limit_ex_encoder_data[0]=0;
gohgwaja 0:7cff999a7f5c 388 }
gohgwaja 0:7cff999a7f5c 389
gohgwaja 0:7cff999a7f5c 390 if(sw2==0 && !limit_find[1])
gohgwaja 0:7cff999a7f5c 391 {
gohgwaja 0:7cff999a7f5c 392 limit_find[1]=true;
gohgwaja 0:7cff999a7f5c 393 encoder2.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 394 limit_ex_encoder_data[1]=0;
gohgwaja 0:7cff999a7f5c 395 }
gohgwaja 0:7cff999a7f5c 396
gohgwaja 0:7cff999a7f5c 397 if(sw3==0 && !limit_find[2])
gohgwaja 0:7cff999a7f5c 398 {
gohgwaja 0:7cff999a7f5c 399 limit_find[2]=true;
gohgwaja 0:7cff999a7f5c 400 encoder3.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 401 limit_ex_encoder_data[2]=0;
gohgwaja 0:7cff999a7f5c 402 }
gohgwaja 0:7cff999a7f5c 403
gohgwaja 0:7cff999a7f5c 404 if(sw4==0 && !limit_find[3])
gohgwaja 0:7cff999a7f5c 405 {
gohgwaja 0:7cff999a7f5c 406 limit_find[3]=true;
gohgwaja 0:7cff999a7f5c 407 encoder4.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 408 limit_ex_encoder_data[3]=0;
gohgwaja 0:7cff999a7f5c 409 }
gohgwaja 0:7cff999a7f5c 410 if(sw5==0 && !limit_find[4])
gohgwaja 0:7cff999a7f5c 411 {
gohgwaja 0:7cff999a7f5c 412 limit_find[4]=true;
gohgwaja 0:7cff999a7f5c 413 encoder5.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 414 limit_ex_encoder_data[4]=0;
gohgwaja 0:7cff999a7f5c 415 }
gohgwaja 0:7cff999a7f5c 416 if(sw6==0 && !limit_find[5])
gohgwaja 0:7cff999a7f5c 417 {
gohgwaja 0:7cff999a7f5c 418 limit_find[5]=true;
gohgwaja 0:7cff999a7f5c 419 encoder6.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 420 limit_ex_encoder_data[5]=0;
gohgwaja 0:7cff999a7f5c 421 }
gohgwaja 0:7cff999a7f5c 422
gohgwaja 0:7cff999a7f5c 423 return (limit_find[0] || !motor_onoff[0]) && (limit_find[1] || !motor_onoff[1]) && (limit_find[2] || !motor_onoff[2]) && (limit_find[3] || !motor_onoff[3]) && (limit_find[4] || !motor_onoff[4]) && (limit_find[5] || !motor_onoff[5]);
gohgwaja 0:7cff999a7f5c 424 }
gohgwaja 0:7cff999a7f5c 425
gohgwaja 0:7cff999a7f5c 426 bool limit_check_done=false;
gohgwaja 0:7cff999a7f5c 427 int limit_check_done_cnt=0;
gohgwaja 0:7cff999a7f5c 428 void find_limit()
gohgwaja 0:7cff999a7f5c 429 {
gohgwaja 0:7cff999a7f5c 430 wait_ms(0);
gohgwaja 0:7cff999a7f5c 431
gohgwaja 0:7cff999a7f5c 432 encoder_reset_cnt();
gohgwaja 0:7cff999a7f5c 433
gohgwaja 0:7cff999a7f5c 434 for(int i=0;i<limit_time;i++)
gohgwaja 0:7cff999a7f5c 435 {
gohgwaja 0:7cff999a7f5c 436 if(limit_check())
gohgwaja 0:7cff999a7f5c 437 {
gohgwaja 0:7cff999a7f5c 438 break;
gohgwaja 0:7cff999a7f5c 439 }
gohgwaja 0:7cff999a7f5c 440 reset_check();
gohgwaja 0:7cff999a7f5c 441 wait_ms(1);
gohgwaja 0:7cff999a7f5c 442
gohgwaja 0:7cff999a7f5c 443 }
gohgwaja 0:7cff999a7f5c 444 encoder_read_raw();
gohgwaja 0:7cff999a7f5c 445 motor_power(0,0);
gohgwaja 0:7cff999a7f5c 446 motor_power(1,0);
gohgwaja 0:7cff999a7f5c 447 motor_power(2,0);
gohgwaja 0:7cff999a7f5c 448 motor_power(3,0);
gohgwaja 0:7cff999a7f5c 449 motor_power(4,0);
gohgwaja 0:7cff999a7f5c 450 motor_power(5,0);
gohgwaja 0:7cff999a7f5c 451 }
gohgwaja 0:7cff999a7f5c 452
gohgwaja 0:7cff999a7f5c 453 #endif