![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
Diff: main.cpp
- Revision:
- 0:6da7d0e457a2
- Child:
- 1:baab5b88a142
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,96 @@ +#include "mbed.h" +#include "omuni.hpp" + +int addr[] = {0x10, 0x12, 0x14}; +int armAddr[] = {0x16, 0x18}; +bool arm = false; + +Serial p(USBTX, USBRX); +Serial pc(PA_11, PA_12); +I2C i2cMaster(D14, D15); +// archan +omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 800, 2.0f, addr, 0.25f, 0.1f); + +float vx, vy, ome; +bool f, pre_f; +char recv[3] = {0}; + +void pc_rx() +{ + char rtemp = pc.getc(); + + if((rtemp & 0b11000000) == 0b00000000) recv[0] = rtemp; + else if((rtemp & 0b11000000) == 0b01000000) recv[1] = rtemp; + else if((rtemp & 0b11000000) == 0b10000000) recv[2] = rtemp; + + float direc = (recv[0] & 0x0f) * 3.141592 / 8.0; + float speed = 0.8 * ((recv[0] & 0b00110000) >> 4); + vx = speed * cos(direc) * -1; + vy = speed * sin(direc) * -1; + + if(recv[2] & 0b1) + { + arm = true; + } + else + { + arm = false; + } + + if(recv[1] & 0b11000) + { + f = false; + ome = ((recv[1] & 0b11000) >> 3) * 3.141592 / 2.0; + if(recv[1] & 0b100000) ome *= -1; + } + else + { + if(recv[1] & 0b11) + { + f = true; +// if(pre_f == false) omuni.reset_theta(); + } + else + { + f = false; + } + ome = (recv[1] & 0b11) * 3.141592 / 2.0; + if((recv[1] & 0b100)) ome *= -1; + } + pre_f = f; +} + +int main() +{ + p.baud(115200); + pc.baud(9600); + pc.printf("Hello!\n"); + pc.attach(pc_rx, Serial::RxIrq); + omuni.set_speed(0.0f, 0.0f); + // archan + omuni.set_pid(0, 3.0f, 0.07f, 0.05f); + omuni.set_pid(1, 3.0f, 0.07f, 0.05f); + omuni.set_pid(2, 3.0f, 0.07f, 0.05f); + + while(1) + { + wait(0.001); + omuni.set_speed(vx, vy, ome, f); + omuni.drive(); + + if(arm) + { + char send = -127; + i2cMaster.write(armAddr[0], &send, 1); + i2cMaster.write(armAddr[1], &send, 1); + } + else + { + char send = 0; + i2cMaster.write(armAddr[0], &send, 1); + i2cMaster.write(armAddr[1], &send, 1); + } + + p.printf("%f\n", omuni.get_theta()); + } +} \ No newline at end of file