API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 19th, 2019
169
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 6.72 KB | None | 0 0
raw download clone embed print report
  1. #include "arduino.h"
  2. #include <libmaple/iwdg.h>
  3. #include <HardwareTimer.h>
  4. #include <Wire.h>
  5. #include <STM32ADC.h>
  6. #include <vl53l0x_class.h>
  7.  
  8. TwoWire WIRE1(2, I2C_FAST_MODE);
  9. VL53L0X sensor_vl53l0x(&WIRE1, PB12, PB13);
  10.  
  11. STM32ADC myADC(ADC1);
  12. STM32ADC myADC2(ADC2);
  13.  
  14. #define PWM_OUT PB6
  15. #define PWM_OUT2 PB7
  16. #define PWM_OUT_INT PB5
  17. #define PWM_OUT2_INT PB8
  18. #define PWM_OUT_EN PB4
  19. #define PWM_OUT2_EN PB9
  20.  
  21. float kp = 14.0f, ki = 0.0, kd = 2.00;
  22. float errSum, dErr, error, lastErr;
  23. float output_pid;
  24.  
  25. uint16_t data_adc1 = 0;
  26. uint16_t data_adc2 = 0;
  27.  
  28. uint8 pins[] = {PA0};
  29. const int maxSamples = 1;
  30. uint16_t dataPoints[maxSamples];
  31.  
  32. uint8 pin2 = PA1;
  33. uint16 dado_adc = 0;
  34. float output_pid2 = 0;
  35.  
  36. uint16_t Alimi = 2000;
  37. uint32_t coun1 = 0;
  38. uint32_t coun2 = 0;
  39. uint32_t timeout_close = 1000;
  40. uint16_t max_det = 500;
  41.  
  42. uint32_t previousMillis = 0;
  43. uint32_t previousMillis2 = 0;
  44. uint32_t previousMillis3 = 0;
  45. uint32_t previousMillis4 = 0;
  46.  
  47. int adc111 = 0;
  48. int st = 0;
  49. float adc111O = 0;
  50. float adc111O2 = 10;
  51. float adc_timeout = 0;
  52. float Algal = 0;
  53. float Algal_T = 0;
  54. //float distance_RW = 0;
  55.  
  56. float distance_R = 0;
  57. uint16_t stack1 = 0;
  58. uint16_t stack2 = 0;
  59.  
  60. uint32_t CT = 0;
  61. void int_func()
  62. {
  63.  
  64. dado_adc = myADC2.getData();
  65. }
  66.  
  67. void setup()
  68. {
  69. Serial.begin(115200);
  70. pinMode(PC13, OUTPUT);
  71. pinMode(PWM_OUT_EN, OUTPUT);
  72. pinMode(PWM_OUT2_EN, OUTPUT);
  73. pinMode(PA0, INPUT_ANALOG);
  74. pinMode(PA1, INPUT_ANALOG);
  75. pinMode(PA2, OUTPUT);
  76. pinMode(PA3, INPUT_PULLDOWN);
  77. pinMode(PA4, OUTPUT);
  78. pinMode(PA5, INPUT_ANALOG);
  79.  
  80. digitalWrite(PWM_OUT_EN, HIGH);
  81. digitalWrite(PWM_OUT2_EN, HIGH);
  82.  
  83. pinMode(PWM_OUT_INT, INPUT);
  84. pinMode(PWM_OUT2_INT, INPUT);
  85. delay(1000);
  86. Serial.println("tongST");
  87. str:
  88. WIRE1.begin();
  89. sensor_vl53l0x.VL53L0X_Off();
  90. int status123;
  91.  
  92. status123 = sensor_vl53l0x.InitSensor(0x10);
  93. //sensor_vl53l0x.VL53L0X_decode_timeout(10);
  94.  
  95. if (status123)
  96. {
  97. Serial.println("Init sensor_vl53l0x failed...");
  98. goto str;
  99. }
  100.  
  101. attachInterrupt(PWM_OUT_INT, ADCRE1, FALLING);
  102. attachInterrupt(PWM_OUT2_INT, ADCRE2, FALLING);
  103. attachInterrupt(PA3, INPUTStatus, FALLING);
  104. Timer1.setMode(TIMER_CH3, TIMER_OUTPUTCOMPARE);
  105. Timer1.setPeriod(1000);
  106. Timer1.setCompare(TIMER_CH3, 1);
  107. Timer1.attachInterrupt(TIMER_CH3, handler_led);
  108.  
  109. HardwareTimer timer1 = HardwareTimer(4);
  110. timer1.setPrescaleFactor(1);
  111. timer1.setOverflow(4499);
  112. pinMode(PWM_OUT, PWM);
  113. pinMode(PWM_OUT2, PWM);
  114.  
  115. delay(1000);
  116.  
  117. myADC.calibrate();
  118. myADC.setSampleRate(ADC_SMPR_1_5);
  119. myADC.setScanMode();
  120. delay(1000);
  121. iwdg_init(IWDG_PRE_4, 100);
  122. nvic_irq_set_priority(NVIC_EXTI_15_10, 0);
  123.  
  124. pwmWrite(PWM_OUT, 0);
  125. pwmWrite(PWM_OUT2, 0);
  126. }
  127.  
  128. void loop()
  129. {
  130. if ((millis() - previousMillis3) > timeout_close && adc111O2 == 900)
  131. {
  132. adc111O2 = 10;
  133. //previousMillis3 = millis();
  134. }
  135. // if (distance_R > max_det && distance_R < max_det + 100 && adc111O > 2500 && adc111O < 2700 && adc111O2 == 2600)
  136. // {
  137. // adc111O2 = 1400;
  138. // }
  139. if (adc111O > 1350 && adc111O < 1550)
  140. {
  141. stack1 = 0;
  142. digitalWrite(PA2, HIGH);
  143. }
  144. else
  145. {
  146. digitalWrite(PA2, LOW);
  147. }
  148. if (adc111O > 2500 && adc111O < 2700)
  149. {
  150. digitalWrite(PA4, HIGH);
  151. }
  152. else
  153. {
  154. digitalWrite(PA4, LOW);
  155. }
  156. // if (Serial.available() > 0)
  157. // {
  158. // char incomingByte = Serial.read();
  159. // if (incomingByte == 'A')
  160. // {
  161. // adc111O2 = 900;
  162. // previousMillis3 = millis();
  163. // }
  164. // if (incomingByte == 'B')
  165. // {
  166. // //adc111O2 = 2600;
  167. // }
  168. // }
  169. if (millis() - previousMillis4 > 10 && adc111O > 2500 && adc111O < 2700)
  170. {
  171. uint32_t distance;
  172. int status;
  173. status = sensor_vl53l0x.GetDistance(&distance);
  174.  
  175. if (status == VL53L0X_ERROR_NONE)
  176. {
  177. distance_R = distance;
  178. //smooth(0.6f, distance, distance_R);
  179. //distance;
  180. }
  181.  
  182. if (distance_R > 200 && distance_R < 400 && stack1 == 0)
  183. {
  184. stack1 = 1;
  185. }
  186. if (distance_R > 500 && distance_R < 700 && stack1 == 1)
  187. {
  188. stack1 = 0;
  189. adc111O2 = 10;
  190. }
  191. // if(distance_R>700&&stack1 == 2){
  192. // stack1 = 0;
  193. // adc111O2 = 1400;
  194. // }
  195. previousMillis4 = millis();
  196. }
  197.  
  198. if (millis() - previousMillis > 5)
  199. {
  200.  
  201. Serial.print(data_adc1);
  202. Serial.print(",");
  203. Serial.print(data_adc2);
  204. Serial.print(",");
  205. Serial.print(Algal);
  206. Serial.print(",");
  207. Serial.print(adc111O2);
  208. Serial.print(",");
  209. Serial.print(distance_R);
  210. Serial.print(",");
  211. Serial.println(stack1);
  212. previousMillis = millis();
  213. }
  214. }
  215. void ADCRE1()
  216. {
  217. data_adc1 = analogRead(PA0);
  218. coun2++;
  219. coun1 = 0;
  220. if (data_adc2 > Alimi && coun2 > 1000)
  221. {
  222. data_adc2 = 0;
  223. coun2 = 0;
  224. }
  225. }
  226. void ADCRE2()
  227. {
  228. data_adc2 = analogRead(PA0);
  229. coun1++;
  230. coun2 = 0;
  231. if (data_adc1 > Alimi && coun1 > 1000)
  232. {
  233. data_adc1 = 0;
  234. coun1 = 0;
  235. }
  236. }
  237. void INPUTStatus()
  238. {
  239. adc111O2 = 900;
  240. previousMillis3 = millis();
  241. }
  242. void handler_led(void)
  243. {
  244. iwdg_feed();
  245. adc111O = smooth(0.9f, analogRead(PA1), adc111O);
  246. Algal = map(adc111O, 1450, 2600, 0, 900);
  247. adc_timeout = smooth(0.1f, analogRead(PA5), adc_timeout);
  248. timeout_close = ((((uint16_t)adc_timeout) >> 8) * 500) + 1000;
  249. myPID();
  250. //output_pid2 = output_pid * ((1.00f - (abs(error) / 1300)) + 0.3f);
  251. }
  252. float limit(float input, int min_limit, int max_limit)
  253. {
  254. if (input > max_limit)
  255. input = max_limit;
  256. if (input < min_limit)
  257. input = min_limit;
  258. return input;
  259. }
  260. float smooth(float alfa, float new_data, float old_data)
  261. {
  262. return (old_data + alfa * (new_data - old_data));
  263. }
  264. void myPID()
  265. {
  266. lastErr = error;
  267.  
  268. if (Algal_T > adc111O2)
  269. {
  270.  
  271. if (Algal_T - adc111O2 < 100)
  272. {
  273. Algal_T = Algal_T - 0.5f;
  274. }
  275. else
  276. {
  277. Algal_T = Algal_T - 1.5f;
  278. }
  279. }
  280.  
  281. if (Algal_T < adc111O2)
  282. {
  283.  
  284. if (adc111O2 - Algal_T < 100)
  285. {
  286. Algal_T = Algal_T + 0.5f;
  287. }
  288. else
  289. {
  290. Algal_T = Algal_T + 1.5f;
  291. }
  292. }
  293.  
  294. error = Algal - Algal_T;
  295.  
  296. dErr = smooth(0.8f,((error - lastErr) * 10.0f),dErr);
  297.  
  298. if (abs(error) <= 5)
  299. {
  300. error = 0;
  301. dErr = 0;
  302. }
  303.  
  304. errSum = limit(errSum + (error * 0.01f), -200, 200);
  305.  
  306.  
  307. output_pid = (kp * error) + (ki * errSum) + (kd * dErr);
  308.  
  309. MOTOR(limit(output_pid, -4000.00, 4000.00));
  310. }
  311. void MOTOR(int y)
  312. {
  313. static float ramp;
  314. if (ramp < 1)
  315. {
  316. ramp += 0.0003f;
  317. }
  318. int x = y * ramp;
  319. if (x < -1)
  320. {
  321. digitalWrite(PC13, HIGH);
  322. if (data_adc1 > Alimi)
  323. {
  324. pwmWrite(PWM_OUT, 10); //
  325. pwmWrite(PWM_OUT2, 0);
  326. }
  327. else
  328. {
  329. pwmWrite(PWM_OUT, abs(x)); //
  330. pwmWrite(PWM_OUT2, 0);
  331. }
  332. }
  333. else if (x > 1)
  334. {
  335. digitalWrite(PC13, HIGH);
  336. if (data_adc2 > Alimi)
  337. {
  338. pwmWrite(PWM_OUT, 0);
  339. pwmWrite(PWM_OUT2, 10);
  340. }
  341. else
  342. {
  343. pwmWrite(PWM_OUT, 0);
  344. pwmWrite(PWM_OUT2, abs(x));
  345. }
  346. }
  347. else
  348. {
  349. digitalWrite(PC13, LOW);
  350. pwmWrite(PWM_OUT, 0);
  351. pwmWrite(PWM_OUT2, 0);
  352. }
  353. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!