u_guessed_it / Mbed 2 deprecated red_rover

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MODSERIAL telemetry-master

main.cpp@4:8ee76f6d32b5, 2015-05-11 (annotated)

Committer:
cheryl_he
Date:
Mon May 11 02:00:33 2015 +0000
Revision:
4:8ee76f6d32b5
Parent:
3:39e6440ccd45
Child:
5:868e652f26af
new red rover code need to tune constants

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cheryl_he 0:453d27750ab3 1 #include "mbed.h"
cheryl_he 1:ff0fc50372c8 2 #include "telemetry.h"
cheryl_he 1:ff0fc50372c8 3 #include "telemetry-mbed.h"
cheryl_he 3:39e6440ccd45 4 #include "MODSERIAL.h"
cheryl_he 1:ff0fc50372c8 5
cheryl_he 3:39e6440ccd45 6 #include "stdlib.h"
cheryl_he 3:39e6440ccd45 7 #include <vector>
cheryl_he 3:39e6440ccd45 8
cheryl_he 4:8ee76f6d32b5 9 //MODSERIAL telemetry_serial(PTA2, PTA1);
cheryl_he 4:8ee76f6d32b5 10
cheryl_he 3:39e6440ccd45 11 MODSERIAL telemetry_serial(USBTX, USBRX);
cheryl_he 4:8ee76f6d32b5 12
cheryl_he 1:ff0fc50372c8 13 telemetry::MbedHal telemetry_hal(telemetry_serial);
cheryl_he 1:ff0fc50372c8 14 telemetry::Telemetry telemetry_obj(telemetry_hal);
cheryl_he 1:ff0fc50372c8 15
cheryl_he 1:ff0fc50372c8 16 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
cheryl_he 4:8ee76f6d32b5 17 telemetry::NumericArray<uint8_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
cheryl_he 1:ff0fc50372c8 18 telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor", "Motor PWM", "%DC", 0);
cheryl_he 0:453d27750ab3 19
cheryl_he 3:39e6440ccd45 20 //Outputs
cheryl_he 4:8ee76f6d32b5 21 DigitalOut led1(LED1);
cheryl_he 3:39e6440ccd45 22 DigitalOut clk(PTD5);
cheryl_he 3:39e6440ccd45 23 DigitalOut si(PTD4);
cheryl_he 3:39e6440ccd45 24 PwmOut motor1(PTA12);
cheryl_he 3:39e6440ccd45 25 PwmOut motor2(PTA4);
cheryl_he 3:39e6440ccd45 26 DigitalOut break1(PTC7);
cheryl_he 3:39e6440ccd45 27 DigitalOut break2(PTC0);
cheryl_he 3:39e6440ccd45 28 PwmOut servo(PTA5);
cheryl_he 3:39e6440ccd45 29
cheryl_he 4:8ee76f6d32b5 30 //Serial bt(PTA2, PTA1);
cheryl_he 3:39e6440ccd45 31 //Serial pc(USBTX, USBRX); // tx, rx
cheryl_he 3:39e6440ccd45 32
cheryl_he 3:39e6440ccd45 33 //Inputs
cheryl_he 3:39e6440ccd45 34 AnalogIn camData(PTC2);
cheryl_he 3:39e6440ccd45 35
cheryl_he 3:39e6440ccd45 36 //Encoder setup and variables
cheryl_he 3:39e6440ccd45 37 InterruptIn interrupt(PTA13);
cheryl_he 3:39e6440ccd45 38
cheryl_he 4:8ee76f6d32b5 39 //Line Tracking Variables --------------------------------
cheryl_he 4:8ee76f6d32b5 40 float ADCdata [128];
cheryl_he 4:8ee76f6d32b5 41 float maxAccum;
cheryl_he 4:8ee76f6d32b5 42 float maxCount;
cheryl_he 4:8ee76f6d32b5 43 float approxPos;
cheryl_he 4:8ee76f6d32b5 44 float prevApproxPos;
cheryl_he 4:8ee76f6d32b5 45 int trackWindow = 30;
cheryl_he 4:8ee76f6d32b5 46 int startWindow;
cheryl_he 4:8ee76f6d32b5 47 int endWindow;
cheryl_he 4:8ee76f6d32b5 48 float maxVal;
cheryl_he 4:8ee76f6d32b5 49 int maxLoc;
cheryl_he 4:8ee76f6d32b5 50
cheryl_he 4:8ee76f6d32b5 51 bool firstTime;
cheryl_he 4:8ee76f6d32b5 52
cheryl_he 4:8ee76f6d32b5 53 //Data Collection
cheryl_he 4:8ee76f6d32b5 54 bool dataCol = false;
cheryl_he 4:8ee76f6d32b5 55 int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime;
cheryl_he 4:8ee76f6d32b5 56
cheryl_he 4:8ee76f6d32b5 57 //Line Crossing variables
cheryl_he 4:8ee76f6d32b5 58 int prevTrackLoc;
cheryl_he 4:8ee76f6d32b5 59 int spaceThresh = 3;
cheryl_he 4:8ee76f6d32b5 60 int widthThresh = 10;
cheryl_he 4:8ee76f6d32b5 61 bool space;
cheryl_he 4:8ee76f6d32b5 62
cheryl_he 4:8ee76f6d32b5 63 //Servo turning parameters
cheryl_he 4:8ee76f6d32b5 64 float straight = 0.00155f;
cheryl_he 4:8ee76f6d32b5 65 float hardLeft = 0.0012f;
cheryl_he 4:8ee76f6d32b5 66 float hardRight = 0.0020f;
cheryl_he 4:8ee76f6d32b5 67 //float hardLeft = 0.0010f;
cheryl_he 4:8ee76f6d32b5 68 //float hardRight = 0.00195f;
cheryl_he 4:8ee76f6d32b5 69
cheryl_he 4:8ee76f6d32b5 70 //Servo Directions
cheryl_he 4:8ee76f6d32b5 71 float currDir;
cheryl_he 4:8ee76f6d32b5 72 float prevDir;
cheryl_he 4:8ee76f6d32b5 73
cheryl_he 4:8ee76f6d32b5 74 // All linescan data for the period the car is running on. To be printed after a set amount of time
cheryl_he 4:8ee76f6d32b5 75 //std::vector<std::vector<int> > frames;
cheryl_he 4:8ee76f6d32b5 76 const int numData = 1000;
cheryl_he 4:8ee76f6d32b5 77 int lineCenters [numData] = {0};
cheryl_he 4:8ee76f6d32b5 78 int times [numData] = {0};
cheryl_he 4:8ee76f6d32b5 79 int loopCtr = 0;
cheryl_he 3:39e6440ccd45 80
cheryl_he 4:8ee76f6d32b5 81 //End of Line Tracking Variables -------------------------
cheryl_he 4:8ee76f6d32b5 82
cheryl_he 4:8ee76f6d32b5 83 //Encoder and Motor Driver Variables ---------------------
cheryl_he 4:8ee76f6d32b5 84
cheryl_he 4:8ee76f6d32b5 85 //Intervals used during encoder data collection to measure velocity
cheryl_he 4:8ee76f6d32b5 86 int interval1=0;
cheryl_he 4:8ee76f6d32b5 87 int interval2=0;
cheryl_he 4:8ee76f6d32b5 88 int interval3=0;
cheryl_he 4:8ee76f6d32b5 89 int avg_interval=0;
cheryl_he 4:8ee76f6d32b5 90 int lastchange1 = 0;
cheryl_he 4:8ee76f6d32b5 91 int lastchange2 = 0;
cheryl_he 4:8ee76f6d32b5 92 int lastchange3 = 0;
cheryl_he 4:8ee76f6d32b5 93 int lastchange4 = 0;
cheryl_he 4:8ee76f6d32b5 94
cheryl_he 4:8ee76f6d32b5 95 //Variables used to for velocity control
cheryl_he 4:8ee76f6d32b5 96 float avg_speed = 0;
cheryl_he 4:8ee76f6d32b5 97 float last_speed = 0;
cheryl_he 4:8ee76f6d32b5 98
cheryl_he 4:8ee76f6d32b5 99 float stall_check = 0;
cheryl_he 4:8ee76f6d32b5 100 float tuning_val = 1;
cheryl_he 3:39e6440ccd45 101
cheryl_he 4:8ee76f6d32b5 102 int numInterrupts = 0;
cheryl_he 4:8ee76f6d32b5 103
cheryl_he 4:8ee76f6d32b5 104 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~
cheryl_he 4:8ee76f6d32b5 105 float pulsewidth = 0.2f; //0.2f
cheryl_he 4:8ee76f6d32b5 106 int intTimMod = 0;
cheryl_he 4:8ee76f6d32b5 107 float maxValThresh = .1; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker
cheryl_he 4:8ee76f6d32b5 108 bool turnSpeedControl = true; //have increased PWMs when turning when true.
cheryl_he 4:8ee76f6d32b5 109 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
cheryl_he 4:8ee76f6d32b5 110
cheryl_he 4:8ee76f6d32b5 111 // Hardware periods
cheryl_he 4:8ee76f6d32b5 112 float motorPeriod = .0018;
cheryl_he 4:8ee76f6d32b5 113 float servoPeriod = .0025;
cheryl_he 4:8ee76f6d32b5 114
cheryl_he 4:8ee76f6d32b5 115 Timer t;
cheryl_he 4:8ee76f6d32b5 116 Timer servoTimer;
cheryl_he 4:8ee76f6d32b5 117 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
cheryl_he 4:8ee76f6d32b5 118
cheryl_he 4:8ee76f6d32b5 119 //End of Encoder and Motor Driver Variables ----------------------
cheryl_he 4:8ee76f6d32b5 120
cheryl_he 4:8ee76f6d32b5 121 //Function for speeding up KL25Z ADC
cheryl_he 3:39e6440ccd45 122 void initADC(void){
cheryl_he 3:39e6440ccd45 123
cheryl_he 3:39e6440ccd45 124 ADC0->CFG1 = ADC0->CFG1 & (
cheryl_he 3:39e6440ccd45 125 ~(
cheryl_he 3:39e6440ccd45 126 0x80 // LDLPC = 0 ; no low-power mode
cheryl_he 3:39e6440ccd45 127 | 0x60 // ADIV = 1
cheryl_he 3:39e6440ccd45 128 | 0x10 // Sample time short
cheryl_he 3:39e6440ccd45 129 | 0x03 // input clock = BUS CLK
cheryl_he 3:39e6440ccd45 130 )
cheryl_he 3:39e6440ccd45 131 ) ; // clkdiv <= 1
cheryl_he 3:39e6440ccd45 132 ADC0->CFG2 = ADC0->CFG2
cheryl_he 3:39e6440ccd45 133 | 0x03 ; // Logsample Time 11 = 2 extra ADCK
cheryl_he 3:39e6440ccd45 134 ADC0->SC3 = ADC0->SC3
cheryl_he 3:39e6440ccd45 135 & (~(0x03)) ; // hardware avarage off
cheryl_he 3:39e6440ccd45 136 }
cheryl_he 0:453d27750ab3 137
cheryl_he 0:453d27750ab3 138 int main() {
cheryl_he 3:39e6440ccd45 139 telemetry_serial.baud(115200);
cheryl_he 1:ff0fc50372c8 140 telemetry_obj.transmit_header();
cheryl_he 3:39e6440ccd45 141
cheryl_he 3:39e6440ccd45 142 //Alter reg values to speed up KL25Z
cheryl_he 3:39e6440ccd45 143 initADC();
cheryl_he 3:39e6440ccd45 144
cheryl_he 4:8ee76f6d32b5 145 //Line Tracker Initializations
cheryl_he 4:8ee76f6d32b5 146 int integrationCounter = 0;
cheryl_he 4:8ee76f6d32b5 147
cheryl_he 4:8ee76f6d32b5 148 //Initial values for directions
cheryl_he 4:8ee76f6d32b5 149 currDir = 0;
cheryl_he 4:8ee76f6d32b5 150 prevDir = 0;
cheryl_he 4:8ee76f6d32b5 151
cheryl_he 4:8ee76f6d32b5 152 // Motor Driver Initializations
cheryl_he 4:8ee76f6d32b5 153 motor1.period(motorPeriod);
cheryl_he 4:8ee76f6d32b5 154 motor2.period(motorPeriod);
cheryl_he 4:8ee76f6d32b5 155
cheryl_he 4:8ee76f6d32b5 156 // Servo Initialization
cheryl_he 4:8ee76f6d32b5 157 servo.period(servoPeriod);
cheryl_he 4:8ee76f6d32b5 158 servo.pulsewidth(hardRight);
cheryl_he 4:8ee76f6d32b5 159 wait(3);
cheryl_he 4:8ee76f6d32b5 160
cheryl_he 4:8ee76f6d32b5 161 motor1.pulsewidth(motorPeriod*pulsewidth);
cheryl_he 4:8ee76f6d32b5 162 motor2.pulsewidth(motorPeriod*pulsewidth);
cheryl_he 4:8ee76f6d32b5 163 break1 = 0;
cheryl_he 4:8ee76f6d32b5 164 break2 = 0;
cheryl_he 4:8ee76f6d32b5 165
cheryl_he 4:8ee76f6d32b5 166 firstTime = true;
cheryl_he 4:8ee76f6d32b5 167
cheryl_he 3:39e6440ccd45 168 t.start();
cheryl_he 1:ff0fc50372c8 169
cheryl_he 4:8ee76f6d32b5 170 if(dataCol){
cheryl_he 4:8ee76f6d32b5 171 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
cheryl_he 4:8ee76f6d32b5 172 printTimer.start();
cheryl_he 4:8ee76f6d32b5 173 }
cheryl_he 4:8ee76f6d32b5 174 //uint16_t* data = camData.read();
cheryl_he 0:453d27750ab3 175 while(1) {
cheryl_he 4:8ee76f6d32b5 176 if(dataCol){
cheryl_he 4:8ee76f6d32b5 177 //break out of main loop if enough time has passed;
cheryl_he 4:8ee76f6d32b5 178 if(loopCtr >= numData && dataCol){
cheryl_he 4:8ee76f6d32b5 179 break;
cheryl_he 4:8ee76f6d32b5 180 }
cheryl_he 4:8ee76f6d32b5 181 }
cheryl_he 4:8ee76f6d32b5 182 if(integrationCounter % 151== 0){
cheryl_he 4:8ee76f6d32b5 183 /*
cheryl_he 4:8ee76f6d32b5 184 //Disable interrupts
cheryl_he 4:8ee76f6d32b5 185 interrupt.fall(NULL);
cheryl_he 4:8ee76f6d32b5 186 interrupt.rise(NULL);
cheryl_he 4:8ee76f6d32b5 187 */
cheryl_he 4:8ee76f6d32b5 188
cheryl_he 4:8ee76f6d32b5 189 //Send start of integration signal
cheryl_he 4:8ee76f6d32b5 190 si = 1;
cheryl_he 4:8ee76f6d32b5 191 clk = 1;
cheryl_he 4:8ee76f6d32b5 192
cheryl_he 4:8ee76f6d32b5 193 si = 0;
cheryl_he 4:8ee76f6d32b5 194 clk = 0;
cheryl_he 4:8ee76f6d32b5 195
cheryl_he 4:8ee76f6d32b5 196 //Reset timing counter for integration
cheryl_he 4:8ee76f6d32b5 197 integrationCounter = 0;
cheryl_he 4:8ee76f6d32b5 198
cheryl_he 4:8ee76f6d32b5 199 //Reset line tracking variables
cheryl_he 4:8ee76f6d32b5 200 maxAccum = 0;
cheryl_he 4:8ee76f6d32b5 201 maxCount = 0;
cheryl_he 4:8ee76f6d32b5 202 approxPos = 0;
cheryl_he 4:8ee76f6d32b5 203
cheryl_he 4:8ee76f6d32b5 204 space = false;
cheryl_he 4:8ee76f6d32b5 205
cheryl_he 1:ff0fc50372c8 206 }
cheryl_he 4:8ee76f6d32b5 207 else if (integrationCounter > 129){
cheryl_he 4:8ee76f6d32b5 208 //Start Timer
cheryl_he 4:8ee76f6d32b5 209 //t.start();
cheryl_he 4:8ee76f6d32b5 210
cheryl_he 4:8ee76f6d32b5 211 //Enable interrupts
cheryl_he 4:8ee76f6d32b5 212 //interrupt.fall(&fallInterrupt);
cheryl_he 4:8ee76f6d32b5 213 //interrupt.rise(&riseInterrupt);
cheryl_he 4:8ee76f6d32b5 214 tele_time_ms = t.read_ms();
cheryl_he 4:8ee76f6d32b5 215 for (uint16_t i=0; i<128; i++) {
cheryl_he 4:8ee76f6d32b5 216 tele_linescan[i] = (uint8_t) (ADCdata[i] * 128);
cheryl_he 4:8ee76f6d32b5 217 }
cheryl_he 4:8ee76f6d32b5 218 //telemetry_obj.do_io();
cheryl_he 4:8ee76f6d32b5 219 if (firstTime){
cheryl_he 4:8ee76f6d32b5 220
cheryl_he 4:8ee76f6d32b5 221 maxVal = ADCdata[10];
cheryl_he 4:8ee76f6d32b5 222 for (int c = 11; c < 118; c++) {
cheryl_he 4:8ee76f6d32b5 223 if (ADCdata[c] > maxVal){
cheryl_he 4:8ee76f6d32b5 224 maxVal = ADCdata[c];
cheryl_he 4:8ee76f6d32b5 225 maxLoc = c;
cheryl_he 4:8ee76f6d32b5 226 }
cheryl_he 4:8ee76f6d32b5 227 }
cheryl_he 4:8ee76f6d32b5 228
cheryl_he 4:8ee76f6d32b5 229 for (int c = 10; c < 118; c++) {
cheryl_he 4:8ee76f6d32b5 230 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
cheryl_he 4:8ee76f6d32b5 231 maxAccum += c;
cheryl_he 4:8ee76f6d32b5 232 maxCount++;
cheryl_he 4:8ee76f6d32b5 233 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
cheryl_he 4:8ee76f6d32b5 234 space = true;
cheryl_he 4:8ee76f6d32b5 235 }
cheryl_he 4:8ee76f6d32b5 236 prevTrackLoc = c;
cheryl_he 4:8ee76f6d32b5 237 }
cheryl_he 4:8ee76f6d32b5 238 }
cheryl_he 4:8ee76f6d32b5 239
cheryl_he 4:8ee76f6d32b5 240 //firstTime = false;
cheryl_he 4:8ee76f6d32b5 241 } else {
cheryl_he 4:8ee76f6d32b5 242
cheryl_he 4:8ee76f6d32b5 243 startWindow = prevApproxPos - trackWindow;
cheryl_he 4:8ee76f6d32b5 244 endWindow = prevApproxPos + trackWindow;
cheryl_he 4:8ee76f6d32b5 245 if (startWindow < 0){
cheryl_he 4:8ee76f6d32b5 246 startWindow = 0;
cheryl_he 4:8ee76f6d32b5 247 }
cheryl_he 4:8ee76f6d32b5 248 if (endWindow > 118){
cheryl_he 4:8ee76f6d32b5 249 endWindow = 118;
cheryl_he 4:8ee76f6d32b5 250 }
cheryl_he 4:8ee76f6d32b5 251 maxVal = ADCdata[10];
cheryl_he 4:8ee76f6d32b5 252 for (int c = startWindow; c < endWindow; c++) {
cheryl_he 4:8ee76f6d32b5 253 if (ADCdata[c] > maxVal){
cheryl_he 4:8ee76f6d32b5 254 maxVal = ADCdata[c];
cheryl_he 4:8ee76f6d32b5 255 maxLoc = c;
cheryl_he 4:8ee76f6d32b5 256 }
cheryl_he 4:8ee76f6d32b5 257 }
cheryl_he 4:8ee76f6d32b5 258
cheryl_he 4:8ee76f6d32b5 259 for (int c = startWindow; c < endWindow; c++) {
cheryl_he 4:8ee76f6d32b5 260 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
cheryl_he 4:8ee76f6d32b5 261 maxAccum += c;
cheryl_he 4:8ee76f6d32b5 262 maxCount++;
cheryl_he 4:8ee76f6d32b5 263 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
cheryl_he 4:8ee76f6d32b5 264 space = true;
cheryl_he 4:8ee76f6d32b5 265 }
cheryl_he 4:8ee76f6d32b5 266 prevTrackLoc = c;
cheryl_he 4:8ee76f6d32b5 267 }
cheryl_he 4:8ee76f6d32b5 268 }
cheryl_he 4:8ee76f6d32b5 269 }
cheryl_he 4:8ee76f6d32b5 270 /*
cheryl_he 4:8ee76f6d32b5 271 //Check if we need to alter integration time due to brightness
cheryl_he 4:8ee76f6d32b5 272 if (maxVal < 0.15f){
cheryl_he 4:8ee76f6d32b5 273 intTimMod += 10;
cheryl_he 4:8ee76f6d32b5 274 } else if (maxVal >= 1) {
cheryl_he 4:8ee76f6d32b5 275 if (intTimMod > 0) {
cheryl_he 4:8ee76f6d32b5 276 intTimMod -= 10;
cheryl_he 4:8ee76f6d32b5 277 }
cheryl_he 4:8ee76f6d32b5 278 }
cheryl_he 4:8ee76f6d32b5 279 */
cheryl_he 4:8ee76f6d32b5 280
cheryl_he 4:8ee76f6d32b5 281 //Line Crossing Checks
cheryl_he 4:8ee76f6d32b5 282 if (space) {
cheryl_he 4:8ee76f6d32b5 283 currDir = prevDir;
cheryl_he 4:8ee76f6d32b5 284 firstTime = true;
cheryl_he 4:8ee76f6d32b5 285 } else {
cheryl_he 4:8ee76f6d32b5 286 approxPos = (float)maxAccum/(float)maxCount;
cheryl_he 4:8ee76f6d32b5 287
cheryl_he 4:8ee76f6d32b5 288 if(dataCol){
cheryl_he 4:8ee76f6d32b5 289 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
cheryl_he 4:8ee76f6d32b5 290 lineCenters[loopCtr] = approxPos;
cheryl_he 4:8ee76f6d32b5 291 times[loopCtr] = printTimer.read_ms();
cheryl_he 4:8ee76f6d32b5 292 loopCtr++;
cheryl_he 4:8ee76f6d32b5 293 }
cheryl_he 4:8ee76f6d32b5 294 }
cheryl_he 4:8ee76f6d32b5 295
cheryl_he 4:8ee76f6d32b5 296 currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft);
cheryl_he 4:8ee76f6d32b5 297 prevApproxPos = approxPos;
cheryl_he 4:8ee76f6d32b5 298
cheryl_he 4:8ee76f6d32b5 299 }
cheryl_he 4:8ee76f6d32b5 300 /*
cheryl_he 4:8ee76f6d32b5 301 if (turnSpeedControl){
cheryl_he 4:8ee76f6d32b5 302 //Change speed when turning at different angles
cheryl_he 4:8ee76f6d32b5 303 if(approxPos > 0 && approxPos <= 45){
cheryl_he 4:8ee76f6d32b5 304 motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
cheryl_he 4:8ee76f6d32b5 305 motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
cheryl_he 4:8ee76f6d32b5 306 } else if (approxPos > 45 && approxPos <= 55){
cheryl_he 4:8ee76f6d32b5 307 motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
cheryl_he 4:8ee76f6d32b5 308 motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
cheryl_he 4:8ee76f6d32b5 309 } else if (approxPos > 55 && approxPos <= 85){
cheryl_he 4:8ee76f6d32b5 310 motor1.pulsewidth(motorPeriod*pulsewidth);
cheryl_he 4:8ee76f6d32b5 311 motor2.pulsewidth(motorPeriod*pulsewidth);
cheryl_he 4:8ee76f6d32b5 312 } else if (approxPos > 85 && approxPos <= 95){
cheryl_he 4:8ee76f6d32b5 313 motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
cheryl_he 4:8ee76f6d32b5 314 motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
cheryl_he 4:8ee76f6d32b5 315 } else {
cheryl_he 4:8ee76f6d32b5 316 motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
cheryl_he 4:8ee76f6d32b5 317 motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
cheryl_he 4:8ee76f6d32b5 318 }
cheryl_he 4:8ee76f6d32b5 319 }
cheryl_he 4:8ee76f6d32b5 320 */
cheryl_he 4:8ee76f6d32b5 321 servo.pulsewidth(currDir);
cheryl_he 4:8ee76f6d32b5 322
cheryl_he 4:8ee76f6d32b5 323 //Start Velocity control after requisite number of encoder signals have been collected
cheryl_he 4:8ee76f6d32b5 324 //if(numInterrupts >= 4){
cheryl_he 4:8ee76f6d32b5 325 //velocity_control(0.1f, TUNING_CONSTANT_10);
cheryl_he 4:8ee76f6d32b5 326 //}
cheryl_he 4:8ee76f6d32b5 327
cheryl_he 4:8ee76f6d32b5 328 //Save current direction as previous direction
cheryl_he 4:8ee76f6d32b5 329 prevDir = currDir;
cheryl_he 4:8ee76f6d32b5 330
cheryl_he 4:8ee76f6d32b5 331 //Prepare to start collecting more data
cheryl_he 4:8ee76f6d32b5 332 integrationCounter = 150;
cheryl_he 4:8ee76f6d32b5 333
cheryl_he 4:8ee76f6d32b5 334 //Disable interrupts
cheryl_he 4:8ee76f6d32b5 335 //interrupt.fall(NULL);
cheryl_he 4:8ee76f6d32b5 336 //interrupt.rise(NULL);
cheryl_he 4:8ee76f6d32b5 337
cheryl_he 4:8ee76f6d32b5 338 //Stop timer
cheryl_he 4:8ee76f6d32b5 339 //t.stop();
cheryl_he 3:39e6440ccd45 340 }
cheryl_he 4:8ee76f6d32b5 341 else{
cheryl_he 4:8ee76f6d32b5 342 clk = 1;
cheryl_he 4:8ee76f6d32b5 343 wait_us(intTimMod);
cheryl_he 4:8ee76f6d32b5 344 ADCdata[integrationCounter - 1] = camData;
cheryl_he 4:8ee76f6d32b5 345 clk = 0;
cheryl_he 4:8ee76f6d32b5 346 }
cheryl_he 4:8ee76f6d32b5 347
cheryl_he 4:8ee76f6d32b5 348 //clk = 0;
cheryl_he 4:8ee76f6d32b5 349 integrationCounter++;
cheryl_he 4:8ee76f6d32b5 350 if(dataCol){
cheryl_he 4:8ee76f6d32b5 351 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
cheryl_he 4:8ee76f6d32b5 352 }
cheryl_he 4:8ee76f6d32b5 353 //camData.
cheryl_he 1:ff0fc50372c8 354 telemetry_obj.do_io();
cheryl_he 4:8ee76f6d32b5 355
cheryl_he 1:ff0fc50372c8 356
cheryl_he 0:453d27750ab3 357 }
cheryl_he 4:8ee76f6d32b5 358 /*
cheryl_he 4:8ee76f6d32b5 359 if (dataCol){
cheryl_he 4:8ee76f6d32b5 360 //print frame data
cheryl_he 4:8ee76f6d32b5 361 pc.printf("printing frame data\n\r");
cheryl_he 4:8ee76f6d32b5 362 //int frameSize = frames.size();
cheryl_he 4:8ee76f6d32b5 363 //pc.printf("%i",frameSize);
cheryl_he 4:8ee76f6d32b5 364 pc.printf("[");
cheryl_he 4:8ee76f6d32b5 365 for(int i=0; i<numData; i++){
cheryl_he 4:8ee76f6d32b5 366 if(lineCenters > 0){
cheryl_he 4:8ee76f6d32b5 367 pc.printf("%i %i,",lineCenters[i], times[i]);
cheryl_he 4:8ee76f6d32b5 368 }
cheryl_he 4:8ee76f6d32b5 369 }
cheryl_he 4:8ee76f6d32b5 370 pc.printf("]\n\r");
cheryl_he 4:8ee76f6d32b5 371 }
cheryl_he 4:8ee76f6d32b5 372 */
cheryl_he 0:453d27750ab3 373 }
cheryl_he 4:8ee76f6d32b5 374