lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

limit.h@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
1:7b5469bf5994 
aaaaaqqqqq

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