From da09baacd218f6604c2077b4d43efc02cb631cc5 Mon Sep 17 00:00:00 2001 From: yikestone Date: Fri, 17 Aug 2018 23:03:18 +0530 Subject: [PATCH] removed imu support, added battery level correction --- config/qt_pi_arduino_params.yaml | 2 +- scripts/qt_pi/arduino_driver.py | 1 - scripts/qt_pi/arduino_driver.pyc | Bin 9854 -> 9825 bytes scripts/qt_pi/base_controller.py | 6 +- scripts/qt_pi/base_controller.pyc | Bin 7142 -> 7189 bytes src/libraries/Arduino/Arduino.ino | 77 ++++++++++++++---------- src/libraries/Arduino/diff_controller.h | 8 ++- 7 files changed, 54 insertions(+), 40 deletions(-) diff --git a/config/qt_pi_arduino_params.yaml b/config/qt_pi_arduino_params.yaml index 5c9d13a..028830b 100644 --- a/config/qt_pi_arduino_params.yaml +++ b/config/qt_pi_arduino_params.yaml @@ -1,5 +1,5 @@ -port: /dev/ttyUSB0 +port: /dev/rfcomm1 baud: 57600 timeout: 0.1 diff --git a/scripts/qt_pi/arduino_driver.py b/scripts/qt_pi/arduino_driver.py index 29f9ac9..6ced6c4 100755 --- a/scripts/qt_pi/arduino_driver.py +++ b/scripts/qt_pi/arduino_driver.py @@ -237,7 +237,6 @@ class Arduino: else: if self.motors_reversed: values[0], values[1] = -values[0], -values[1] - print values return values def reset_encoders(self): diff --git a/scripts/qt_pi/arduino_driver.pyc b/scripts/qt_pi/arduino_driver.pyc index 90d4fb6e1aa88486811ab5d2ddefe362b5b0f6c0..f7c8284453bdb229e81048e0a9d9797753febb61 100644 GIT binary patch delta 683 zcmez8^U#N#`7-29GGdW1o&15DWpgQ)8CJ>7AGl?)N=)9($GJJ0R~oD2=G}Z9SRK8& zL$DB2$A2ly%}&C#7&m3N$*45>pllUZ^$l`DjGrb?mb;BjmWvf=P)bo| zS?c%Ad*#nybCV?>SUx^Cz96+IzWCqd2a0Xji~_|CYjH__0q5i`N)gyp2=V|eOHVC{ zPf9E)iO(s^;hmhMyb_B_lga)nx{TbLYgKBoxyW>~y_%XN8xJr5IGNahP@0j8iItI; KiG_&^$N~VrJk4_e delta 671 zcmaFp^UsH!`7KHe@lK9M8TP zQ$mqrKQ=+r%_^KMjF>V&b2jI2S>cd*#Vw0f#&WYiuL5?N$^N{ooA>b5V|VlBdci`h z`b;;Q2sh%;^HcODrp)Gt5=@w);BZlwK8NX1MGn)+mNM#$8Iu!b5}DjRChwQ2!c+wc z)>>IHq)$ diff --git a/scripts/qt_pi/base_controller.py b/scripts/qt_pi/base_controller.py index 070a4ca..1de8693 100755 --- a/scripts/qt_pi/base_controller.py +++ b/scripts/qt_pi/base_controller.py @@ -211,9 +211,9 @@ class BaseController: self.arduino.drive(self.v_left, self.v_right) self.t_next = now + self.t_delta - self.batt_lvl = self.arduino.get_batt_lvl() - - self.batt_lvl_pub.publish(((self.batt_lvl) / 5) * 5 * 12.85 / 900) + self.batt_lvl = self.arduino.get_batt_lvl() / 100.0 + + self.batt_lvl_pub.publish(self.batt_lvl) def stop(self): self.stopped = True self.arduino.drive(0, 0) diff --git a/scripts/qt_pi/base_controller.pyc b/scripts/qt_pi/base_controller.pyc index d1a3d5a542a31195b55cba95a0e30e49959940c2..bee08286f747eccef87d865d72f8a5fd4188832c 100644 GIT binary patch delta 2355 zcmZuy&2Jl35TCWZcAUg^5<51Io9{2@)A^2*w2ccbZA78f3RR*KT9WKG$tKv1{gQ^t z(H>Ie0NkE9ATH&`1ymHo6^TE93l}bxS|M>km5>k@PRz`DHwme;`f1*8zTV86_wvX5 z7x|F???`=Vx$=GJm7)mwkMfais89va8$dL|5x}7eLIo=b27Vzp@hiPI$(xV5styZzScpWIWv^^pH7!tD?#q z{uw*sZoCgfm?L3XQJ3`gf@F*;wwkE2BLx&%G&9u#A`XdG>PLc!2x9^gWD^3`(3XhS zlN>t*)*I9~`!EPq{7{i*MH^LL^TRZx$kZ&X45Vm{6#y$bT%pqq-sKA2uF&HOy{^#b z3jHY99B9DZ!yp*_%w%~pLtqSm7$(LVfg`_DH7-VJ?+P967#M>f#=*#fF@*i2zUVpj zM%6Hg309%EqnZRUMb2b44dR}+m|>q;Fh)VlamIP-bIKTlRE=nMh;c~K0Z?`{$6Ss% zjvbj#kh#3AuBlsqKlz>d+sS9aqN3#GzoFxX5OSO5sxZWWXY1DQa~xU zS^C(_eX_M}R%9VLCnF8b@dsq0glm5prBw-^D!g<1BMXxxGR4WD}bsJL|urA5{rruDRNUKdg zZA#efbYqWH=a4(l0t}3bcj3&YDPAXsXlGN=#ic~awHgJh8}y!<8kc%6m+0_)%(t*v zKUnw;&~7OrUYjV`L^*5%4F7pM-X|qs8+{O}VK&Huh2IdD?YMXh2CYA1GfX#(l8w;# zVknxS+5wDFjkYz;`W(*@oyf7Dwt?Np8Fqw8>No|NVw`|&fbuVXWCAs68s>evqxEbs zloBAX363@eyGo!qP4cN%-e>>c4MU%ItL2{I+>@Mph2jP*hvM9h=-FWID(9Z$+%vFS z^>Vvjd7qA=jT`jbF!y=4T5g=cuQ?pf=q&HUsd9#%PKAt|k)Cs&D()K1Gu~wYqtST_ z^X$05=YUD`d=y2w6u%M^3f`BkjXTx9KD_^8U|Al-hr%03;qQZj{2{(*FQTaR5KIuz z_0?tx+7Uzx!iFhyv$$R|%(A{-+AE5RmLpvdVW+fVmdn~ZC~8wKDN1_9T-WA_oFm8+ zj1fc$d<3*HZ2>TqwnQTBL(pqt9+>NU!qn*f&;|+cq5~K5nDLQzOMafnC@b=t#HaS# zq$IzGduuyel})XHvNlc7PjHoBncyBIE{&S3Z6aj3AC!dOO@`Mzo{MTllpa1m`8_qRbeUdf|>#S)Aa2#F0A!Bm@YFTXR9-%*?EJlVFL&{xbhJuYcyB|Id@sUu!}6 z?{MelE$Mmig(U5ZQ|W!%>Vji{BR^PvFax0bK@Y&O4;m;0;c|=ZILC%Ow(}g@<+0tM zD{h3a$3{T!aoMQH_6~sF=h88cj)R^M|M=!|o=e8l4W9ED^4MYWxGAsL-EJY1V1+m` zfEIR1Z*NFOpaEd^P^HHbNVJGD)eBZ1#1lwpF($ehV-P2sAh3qENVJyV*hw%`)JXd% z01f=0qMsF5pu+CxhiOQWZdsUVNYOe2I+>YnB-E5**7;)wr0Wy-Rf1^Pr<`4s^7g#r{!Wv^;yG>JnH5R-uEV zS^#U2oXKnntarV|GW)E6J`dI^XDm{mQ+f_kEh6d=c}UR#P;K7b0RA+;2vj6#O~@UOn@>7QzL5nav!r3DZ=zJVUwu$6RP|cb(8Xc0INpJzrWqB( z(zG6w>)3RxIn?^iNQvgvu8(|yw~$re7T<^GqHS^h7%2MZP=801A{){#;^WAO^kegB z{aDplTR)Sh|6sGId2!qz=3URP{xi>Ky6CMy$CxsnN zbf#F*59WY)7M+++k|z$?t^yXGm;|P;)pwjDy0d=gG&AJzBAT)X*+0z+Ylt_1mk?(& z0_HGiBP}&9wNYr`Xgd5D^L@M=2NORNv|$>V_(c-0fP9hRJyIO>vd2N2ghq%hrohC{ zG*|BUcnk*ZJZCe5<#Vzum^i2r=Wg=F!I7oP=U6?@)3L8WtuE6x@NjWx9U)IWw`s=( zw#~slKR!24Q{N=jqDxv;tzN#T z7KvOXSReQm>2O z;-A@9$s~yI@PnNLqkdm~m2^u4Qv|ONTp@UwAV+YSU(2+9aqZU=?`|EF+WMEe!#=VqcmFICP?g=^lSVBFDrB;6(8en(NV z@MyuPSk5s}UM}I*8?R~V=IyIzZOYQIa-r-}D<8H_Njx7IYp;}E)h+b+RSc%LN;k-t w9$B?cfHxjg(w-H?Qu`y1VZ;yVsQ+3h6Eei_>4Nl*h-RL)=fnR7;v_TkFR9$G9{>OV diff --git a/src/libraries/Arduino/Arduino.ino b/src/libraries/Arduino/Arduino.ino index 43743cd..933fa3d 100644 --- a/src/libraries/Arduino/Arduino.ino +++ b/src/libraries/Arduino/Arduino.ino @@ -12,14 +12,17 @@ #include "diff_controller.h" #include #include "I2Cdev.h" -#include "MPU9150.h" +#include "AK8975.h" +#include "MPU6050.h" #include "Kalman.h" + Kalman filter; -MPU9150 accelGyroMag; +AK8975 mag(0x0C); +MPU6050 accelgyro; float batt_lvl; int16_t gx; -float mx, my, mz; +int16_t mx, my, mz; int dt; @@ -37,6 +40,7 @@ char argv2[16]; long arg1; long arg2; float heading = 0.0; + void resetCommand() { cmd = NULL; memset(argv1, 0, sizeof(argv1)); @@ -54,12 +58,12 @@ int runCommand() { int pid_args[4]; arg1 = atoi(argv1); arg2 = atoi(argv2); - + switch(cmd) { case GET_BAUDRATE: Serial.println(BAUDRATE); break; - + case READ_ENCODERS: Serial.print(readEncoder(LEFT)); Serial.print(" "); @@ -85,7 +89,7 @@ int runCommand() { rightPID.TargetTicksPerFrame = arg2; Serial.println("OK"); break; - + case UPDATE_PID_L: while ((str = strtok_r(p, ":", &p)) != '\0') { pid_args[i] = atoi(str); @@ -122,10 +126,10 @@ int runCommand() { Serial.println("OK"); break; case BATTERY_LVL: - Serial.println(batt_lvl); + Serial.println((int)(batt_lvl * 100)); break; case IMU: - Serial.println(heading); + Serial.println((int)(heading * 100)); break; default: Serial.println("Invalid Command"); @@ -136,17 +140,25 @@ int runCommand() { /* Setup function--runs once at startup. */ void setup() { Wire.setClock(400000); - Wire.begin(); + Wire.begin(); Serial.begin(BAUDRATE); - accelGyroMag.initialize(); + + accelgyro.initialize(); + accelgyro.setI2CBypassEnabled(true); + mag.initialize(); + mag.testConnection(); delay(100); - accelGyroMag.getHadjusted(&my, &mx, &mz); + mag.getHeading(&my, &mx, &mz); + delay(10); + mag.getHeading(&my, &mx, &mz); + heading = atan2(-mz, my) * RAD_TO_DEG; + filter.setQangle(0.0001); filter.setQbias(0.003); filter.setRmeasure(0.05); filter.setAngle(heading); - + pinMode(A0,INPUT); digitalWrite(A0,1); batt_lvl = analogRead(A0); @@ -154,53 +166,55 @@ void setup() { DDRC &= ~(1< 90) || (tempY > 90 && heading < -90)) { filter.setAngle(tempY); heading = tempY; } - + else - + heading = filter.getAngle(tempY, gx / 131.0, dt); - + //Serial.println(heading); - - + + while (Serial.available() > 0) { - + // Read the next character chr = Serial.read(); @@ -238,18 +252,17 @@ void loop() { } } } - + if (millis() > nextPID) { updatePID(); nextPID += PID_INTERVAL; } - + // Check to see if we have exceeded the auto-stop interval if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {; setMotorSpeeds(0, 0); moving = 0; resetPID(); } - -} +} diff --git a/src/libraries/Arduino/diff_controller.h b/src/libraries/Arduino/diff_controller.h index 25570b6..21da20b 100644 --- a/src/libraries/Arduino/diff_controller.h +++ b/src/libraries/Arduino/diff_controller.h @@ -45,10 +45,12 @@ void doPID(SetPointInfo * p) { input = p->Encoder - p->PrevEnc; Perror = p->TargetTicksPerFrame - input; p->ITerm += (p->Ki) * Perror; - output = p->base_pwm + ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko); - p->PrevEnc = p->Encoder; + output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko); - //output += p->output; + if(p->TargetTicksPerFrame > 0) output += p->base_pwm; + else if(p->TargetTicksPerFrame < 0) output -= p->base_pwm; + + p->PrevEnc = p->Encoder; if (output >= MAX_PWM) output = MAX_PWM;