stuff
Dependencies: mbed QEI BNO055 MBed_Adafruit-GPS-Library
main.cpp@2:825571dce613, 2019-04-23 (annotated)
- Committer:
- LukeMar
- Date:
- Tue Apr 23 17:52:27 2019 +0000
- Revision:
- 2:825571dce613
- Parent:
- 1:4b4d5d18fc57
4/23/19
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LukeMar | 0:f4edd3407cc5 | 1 | |
LukeMar | 0:f4edd3407cc5 | 2 | /* |
LukeMar | 0:f4edd3407cc5 | 3 | Sailbot Hull 14 |
LukeMar | 0:f4edd3407cc5 | 4 | L Marino and D Robinson, |
LukeMar | 0:f4edd3407cc5 | 5 | with D Evangelista, P Frontera, M Kutzer, A Laun |
LukeMar | 0:f4edd3407cc5 | 6 | 2018 |
LukeMar | 0:f4edd3407cc5 | 7 | |
LukeMar | 0:f4edd3407cc5 | 8 | Threaded version using mbed OS5 |
LukeMar | 0:f4edd3407cc5 | 9 | Serial links between nodes |
LukeMar | 0:f4edd3407cc5 | 10 | Stepper motors with Pololu 2968 drivers (MP6500 based) |
LukeMar | 0:f4edd3407cc5 | 11 | Piher PST360 angle sensors |
LukeMar | 0:f4edd3407cc5 | 12 | Adafruit Absolute GPS |
LukeMar | 0:f4edd3407cc5 | 13 | Telemetry via ???Ubiquity??? |
LukeMar | 0:f4edd3407cc5 | 14 | */ |
LukeMar | 0:f4edd3407cc5 | 15 | |
LukeMar | 0:f4edd3407cc5 | 16 | //Thread for logging time and Gps, heading and encoder values is mostly complete |
LukeMar | 0:f4edd3407cc5 | 17 | //control thread is a work in progress |
LukeMar | 0:f4edd3407cc5 | 18 | |
LukeMar | 0:f4edd3407cc5 | 19 | #include "mbed.h" |
LukeMar | 2:825571dce613 | 20 | #include <stdio.h> |
LukeMar | 2:825571dce613 | 21 | #include <stdlib.h> |
LukeMar | 2:825571dce613 | 22 | #include <QEI.h> |
LukeMar | 2:825571dce613 | 23 | |
LukeMar | 2:825571dce613 | 24 | DigitalOut led1(LED1); |
LukeMar | 2:825571dce613 | 25 | DigitalOut led2(LED2); |
LukeMar | 2:825571dce613 | 26 | DigitalOut spin(p20); |
LukeMar | 2:825571dce613 | 27 | DigitalOut fire(p19); |
LukeMar | 2:825571dce613 | 28 | PwmOut pwm(p25); |
LukeMar | 2:825571dce613 | 29 | DigitalOut dir(p27); |
LukeMar | 2:825571dce613 | 30 | Serial pc(USBTX,USBRX); |
LukeMar | 2:825571dce613 | 31 | QEI turret(p17, p18, NC, 1600); |
LukeMar | 2:825571dce613 | 32 | Timer t; |
LukeMar | 0:f4edd3407cc5 | 33 | |
LukeMar | 2:825571dce613 | 34 | int qq; |
LukeMar | 2:825571dce613 | 35 | float pulses; |
LukeMar | 2:825571dce613 | 36 | float revs; |
LukeMar | 2:825571dce613 | 37 | float dc; |
LukeMar | 2:825571dce613 | 38 | float theta; |
LukeMar | 2:825571dce613 | 39 | float err = 0.0; |
LukeMar | 2:825571dce613 | 40 | float err_old = 0.0; |
LukeMar | 2:825571dce613 | 41 | float err_ancient = 0.0; |
LukeMar | 2:825571dce613 | 42 | float dc_old = 0.0; |
LukeMar | 2:825571dce613 | 43 | float dc_ancient = 0.0; |
LukeMar | 2:825571dce613 | 44 | float des_theta=0.0; |
LukeMar | 2:825571dce613 | 45 | float t_now; |
LukeMar | 2:825571dce613 | 46 | int ii=0; |
LukeMar | 2:825571dce613 | 47 | int shots; |
LukeMar | 0:f4edd3407cc5 | 48 | int i; |
LukeMar | 2:825571dce613 | 49 | |
LukeMar | 2:825571dce613 | 50 | //float t; |
LukeMar | 2:825571dce613 | 51 | //float t_2; |
LukeMar | 2:825571dce613 | 52 | int yes; |
LukeMar | 2:825571dce613 | 53 | |
LukeMar | 2:825571dce613 | 54 | Ticker Controller; |
LukeMar | 2:825571dce613 | 55 | void CtrCode() |
LukeMar | 2:825571dce613 | 56 | { |
LukeMar | 2:825571dce613 | 57 | pulses = turret.getPulses(); |
LukeMar | 2:825571dce613 | 58 | revs = -pulses / 3200.0; |
LukeMar | 2:825571dce613 | 59 | theta = revs*360.0; |
LukeMar | 2:825571dce613 | 60 | |
LukeMar | 2:825571dce613 | 61 | err = des_theta-theta; |
LukeMar | 2:825571dce613 | 62 | |
LukeMar | 2:825571dce613 | 63 | //dc = 1.9703*dc_old - 0.9703*dc_ancient + .1471*err- 0.293*err_old + 0.146*err_ancient; //1.9703*dc_old - 0.9703*dc_ancient + .1471*err - 0.293*err_old + 0.146*err_ancient; |
LukeMar | 2:825571dce613 | 64 | dc = 1.9703*dc_old - 0.9703*dc_ancient + 0.7918*err - 1.5762*err_old + 0.7844*err_ancient; |
LukeMar | 0:f4edd3407cc5 | 65 | |
LukeMar | 2:825571dce613 | 66 | err_ancient = err_old; |
LukeMar | 2:825571dce613 | 67 | err_old = err; |
LukeMar | 2:825571dce613 | 68 | dc_ancient = dc_old; |
LukeMar | 2:825571dce613 | 69 | dc_old = dc; |
LukeMar | 2:825571dce613 | 70 | |
LukeMar | 2:825571dce613 | 71 | if (dc>0) { |
LukeMar | 2:825571dce613 | 72 | dir = 1; |
LukeMar | 2:825571dce613 | 73 | } else { |
LukeMar | 2:825571dce613 | 74 | dir = 0; |
LukeMar | 2:825571dce613 | 75 | } |
LukeMar | 2:825571dce613 | 76 | |
LukeMar | 2:825571dce613 | 77 | //Filter for derivative |
LukeMar | 2:825571dce613 | 78 | if(t.read()<=3.0) { |
LukeMar | 2:825571dce613 | 79 | pwm.write(abs(dc)); |
LukeMar | 2:825571dce613 | 80 | } |
LukeMar | 2:825571dce613 | 81 | if(t.read()>3.0) { |
LukeMar | 2:825571dce613 | 82 | if(abs(err)<=0.50) { |
LukeMar | 2:825571dce613 | 83 | pwm.write(0); |
LukeMar | 2:825571dce613 | 84 | } else { |
LukeMar | 2:825571dce613 | 85 | pwm.write(abs(dc)); |
LukeMar | 2:825571dce613 | 86 | } |
LukeMar | 2:825571dce613 | 87 | } |
LukeMar | 2:825571dce613 | 88 | } |
LukeMar | 1:4b4d5d18fc57 | 89 | |
LukeMar | 0:f4edd3407cc5 | 90 | |
LukeMar | 2:825571dce613 | 91 | void FireCode() |
LukeMar | 1:4b4d5d18fc57 | 92 | { |
LukeMar | 2:825571dce613 | 93 | |
LukeMar | 2:825571dce613 | 94 | while(1) { |
LukeMar | 2:825571dce613 | 95 | int counter = 0; |
LukeMar | 2:825571dce613 | 96 | while(abs(err)<=0.50) { |
LukeMar | 2:825571dce613 | 97 | Thread::wait(5); |
LukeMar | 2:825571dce613 | 98 | ii++; |
LukeMar | 2:825571dce613 | 99 | if(ii>15) { |
LukeMar | 0:f4edd3407cc5 | 100 | |
LukeMar | 2:825571dce613 | 101 | spin = 1; |
LukeMar | 2:825571dce613 | 102 | led1 = 1; |
LukeMar | 2:825571dce613 | 103 | Thread::wait(3000); |
LukeMar | 2:825571dce613 | 104 | |
LukeMar | 2:825571dce613 | 105 | while(counter<shots) { |
LukeMar | 2:825571dce613 | 106 | fire = 1; |
LukeMar | 2:825571dce613 | 107 | led2 = 1; |
LukeMar | 2:825571dce613 | 108 | Thread::wait(100); |
LukeMar | 2:825571dce613 | 109 | led2 = 0; |
LukeMar | 2:825571dce613 | 110 | fire = 0; |
LukeMar | 2:825571dce613 | 111 | Thread::wait(7500); |
LukeMar | 2:825571dce613 | 112 | counter = counter + 1; |
LukeMar | 2:825571dce613 | 113 | |
LukeMar | 2:825571dce613 | 114 | } |
LukeMar | 2:825571dce613 | 115 | spin = 0; |
LukeMar | 2:825571dce613 | 116 | led1 = 0; |
LukeMar | 2:825571dce613 | 117 | } |
LukeMar | 2:825571dce613 | 118 | } |
LukeMar | 0:f4edd3407cc5 | 119 | } |
LukeMar | 0:f4edd3407cc5 | 120 | } |
LukeMar | 0:f4edd3407cc5 | 121 | |
LukeMar | 0:f4edd3407cc5 | 122 | // *****threading***** |
LukeMar | 2:825571dce613 | 123 | Thread Spin_thread; |
LukeMar | 2:825571dce613 | 124 | Thread Shoot_thread; |
LukeMar | 0:f4edd3407cc5 | 125 | |
LukeMar | 2:825571dce613 | 126 | |
LukeMar | 2:825571dce613 | 127 | void Spin_callback(void); |
LukeMar | 2:825571dce613 | 128 | void Shoot_callback(void); |
LukeMar | 0:f4edd3407cc5 | 129 | |
LukeMar | 0:f4edd3407cc5 | 130 | int main() |
LukeMar | 0:f4edd3407cc5 | 131 | { |
LukeMar | 2:825571dce613 | 132 | |
LukeMar | 2:825571dce613 | 133 | float pwm_now; |
LukeMar | 2:825571dce613 | 134 | |
LukeMar | 2:825571dce613 | 135 | pc.baud(9600); // set baud rate |
LukeMar | 2:825571dce613 | 136 | dir=1; |
LukeMar | 2:825571dce613 | 137 | pwm.period(1.0/(20*10^3)); |
LukeMar | 2:825571dce613 | 138 | pc.scanf("%f,%d",&des_theta,&shots); //scan in desired theta and # of shots from Tera term |
LukeMar | 2:825571dce613 | 139 | qq = 0; |
LukeMar | 2:825571dce613 | 140 | while(qq<shots) { |
LukeMar | 2:825571dce613 | 141 | led2 = 1; |
LukeMar | 2:825571dce613 | 142 | Thread::wait(750); |
LukeMar | 2:825571dce613 | 143 | led2 = 0; |
LukeMar | 2:825571dce613 | 144 | qq++; |
LukeMar | 2:825571dce613 | 145 | } |
LukeMar | 2:825571dce613 | 146 | t.start(); |
LukeMar | 0:f4edd3407cc5 | 147 | //start all threads |
LukeMar | 2:825571dce613 | 148 | Thread::wait(1000); |
LukeMar | 2:825571dce613 | 149 | Spin_thread.start(callback(Spin_callback)); |
LukeMar | 2:825571dce613 | 150 | Shoot_thread.start(callback(Shoot_callback)); |
LukeMar | 2:825571dce613 | 151 | |
LukeMar | 2:825571dce613 | 152 | while(t.read()<=25) { |
LukeMar | 2:825571dce613 | 153 | t_now = t.read(); |
LukeMar | 2:825571dce613 | 154 | pc.printf("Time: %f, Error: %f, DC: %f\n", t_now,err, dc); |
LukeMar | 2:825571dce613 | 155 | } |
LukeMar | 2:825571dce613 | 156 | |
LukeMar | 2:825571dce613 | 157 | t.stop(); |
LukeMar | 0:f4edd3407cc5 | 158 | //could have a heart beat but I tried to reduce the number of threads fighting for time |
LukeMar | 0:f4edd3407cc5 | 159 | }//end main |
LukeMar | 0:f4edd3407cc5 | 160 | |
LukeMar | 2:825571dce613 | 161 | void Spin_callback(void) |
LukeMar | 0:f4edd3407cc5 | 162 | { |
LukeMar | 2:825571dce613 | 163 | |
LukeMar | 2:825571dce613 | 164 | Controller.attach(&CtrCode,.0083); |
LukeMar | 0:f4edd3407cc5 | 165 | } |
LukeMar | 0:f4edd3407cc5 | 166 | |
LukeMar | 2:825571dce613 | 167 | void Shoot_callback(void) |
LukeMar | 0:f4edd3407cc5 | 168 | { |
LukeMar | 2:825571dce613 | 169 | FireCode(); |
LukeMar | 1:4b4d5d18fc57 | 170 | |
LukeMar | 2:825571dce613 | 171 | } |