Matthew Marino / Mbed 2 deprecated Threaded_GUNCONT

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?

UserRevisionLine numberNew 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 }