ELCT 302 / Mbed 2 deprecated camera_steering

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

main.cpp@0:2b269b9f417b, 2018-04-18 (annotated)

Committer:
KDrainEE
Date:
Wed Apr 18 22:14:31 2018 +0000
Revision:
0:2b269b9f417b
This bitch works with just the camera. The checkpoints throw it off some though.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KDrainEE 0:2b269b9f417b 1 #include "mbed.h"
KDrainEE 0:2b269b9f417b 2 #include <iostream>
KDrainEE 0:2b269b9f417b 3
KDrainEE 0:2b269b9f417b 4 #define KPS 2.0E-2f
KDrainEE 0:2b269b9f417b 5 #define KD 1.0e-4f
KDrainEE 0:2b269b9f417b 6 #define TI 0.001f
KDrainEE 0:2b269b9f417b 7 #define SET 0.0f
KDrainEE 0:2b269b9f417b 8 #define MINS 0.05f
KDrainEE 0:2b269b9f417b 9 #define MAXS 0.1f
KDrainEE 0:2b269b9f417b 10 #define BIAS 0.075f
KDrainEE 0:2b269b9f417b 11 #define TOL 0.02f
KDrainEE 0:2b269b9f417b 12 #define STEER_FREQ 0.02f //50 Hz
KDrainEE 0:2b269b9f417b 13 #define STEERUPDATEPERIOD 0.05
KDrainEE 0:2b269b9f417b 14
KDrainEE 0:2b269b9f417b 15 AnalogIn cameraIn1(PTB0);
KDrainEE 0:2b269b9f417b 16 DigitalOut si(PTA12);
KDrainEE 0:2b269b9f417b 17 DigitalOut clk(PTD4);
KDrainEE 0:2b269b9f417b 18
KDrainEE 0:2b269b9f417b 19 int sensorVal = 0;
KDrainEE 0:2b269b9f417b 20 float PixArray[128];
KDrainEE 0:2b269b9f417b 21 int maxVal = 9;
KDrainEE 0:2b269b9f417b 22
KDrainEE 0:2b269b9f417b 23 PwmOut servoSig(PTD2); //PWM output to control servo
KDrainEE 0:2b269b9f417b 24 DigitalOut myled(LED_BLUE);
KDrainEE 0:2b269b9f417b 25 Ticker control;
KDrainEE 0:2b269b9f417b 26 Timer ctrlTimer;
KDrainEE 0:2b269b9f417b 27
KDrainEE 0:2b269b9f417b 28 float data[4];
KDrainEE 0:2b269b9f417b 29
KDrainEE 0:2b269b9f417b 30 float fbPrev = 0.0f;
KDrainEE 0:2b269b9f417b 31 Serial bt(PTE0, PTE1); //COM12
KDrainEE 0:2b269b9f417b 32
KDrainEE 0:2b269b9f417b 33 void steer()
KDrainEE 0:2b269b9f417b 34 {
KDrainEE 0:2b269b9f417b 35
KDrainEE 0:2b269b9f417b 36 float fb = ((float)maxVal)/64.0;
KDrainEE 0:2b269b9f417b 37 float e = SET - fb;
KDrainEE 0:2b269b9f417b 38 float Controlleroutput = KPS * e - (KD * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error??
KDrainEE 0:2b269b9f417b 39 if (Controlleroutput > MAXS) Controlleroutput = MAXS;
KDrainEE 0:2b269b9f417b 40 else if (Controlleroutput < MINS) Controlleroutput = MINS;
KDrainEE 0:2b269b9f417b 41 if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD)
KDrainEE 0:2b269b9f417b 42 {
KDrainEE 0:2b269b9f417b 43 ctrlTimer.reset();
KDrainEE 0:2b269b9f417b 44 servoSig.write(Controlleroutput);
KDrainEE 0:2b269b9f417b 45 }
KDrainEE 0:2b269b9f417b 46 fbPrev = fb;
KDrainEE 0:2b269b9f417b 47 data[2] = Controlleroutput;
KDrainEE 0:2b269b9f417b 48 data[3] = e;
KDrainEE 0:2b269b9f417b 49 }
KDrainEE 0:2b269b9f417b 50
KDrainEE 0:2b269b9f417b 51 int main()
KDrainEE 0:2b269b9f417b 52 {
KDrainEE 0:2b269b9f417b 53 //startup checks
KDrainEE 0:2b269b9f417b 54 servoSig.period(STEER_FREQ);
KDrainEE 0:2b269b9f417b 55 servoSig.write(0.05);
KDrainEE 0:2b269b9f417b 56 wait(0.5);
KDrainEE 0:2b269b9f417b 57 servoSig.write(0.1);
KDrainEE 0:2b269b9f417b 58 wait(0.5);
KDrainEE 0:2b269b9f417b 59 servoSig.write(0.075);
KDrainEE 0:2b269b9f417b 60 wait(0.5);
KDrainEE 0:2b269b9f417b 61
KDrainEE 0:2b269b9f417b 62 //Line-Scan camera
KDrainEE 0:2b269b9f417b 63 //Initialize -- Clocks out indeterminate pixel data from power up
KDrainEE 0:2b269b9f417b 64 si.write(1);
KDrainEE 0:2b269b9f417b 65 clk.write(1);
KDrainEE 0:2b269b9f417b 66 si.write(0);
KDrainEE 0:2b269b9f417b 67 clk.write(0);
KDrainEE 0:2b269b9f417b 68
KDrainEE 0:2b269b9f417b 69 for (int i = 0; i< 128; i++)
KDrainEE 0:2b269b9f417b 70 {
KDrainEE 0:2b269b9f417b 71 clk.write(1);
KDrainEE 0:2b269b9f417b 72 clk.write(0);
KDrainEE 0:2b269b9f417b 73 }
KDrainEE 0:2b269b9f417b 74 //End Initializatio
KDrainEE 0:2b269b9f417b 75
KDrainEE 0:2b269b9f417b 76 ctrlTimer.start();
KDrainEE 0:2b269b9f417b 77 control.attach(&steer, TI);
KDrainEE 0:2b269b9f417b 78 bt.baud(115200);
KDrainEE 0:2b269b9f417b 79 while(1) {
KDrainEE 0:2b269b9f417b 80 sensorVal = cameraIn1.read();
KDrainEE 0:2b269b9f417b 81
KDrainEE 0:2b269b9f417b 82 //Start pixel count
KDrainEE 0:2b269b9f417b 83 si.write(1);
KDrainEE 0:2b269b9f417b 84 clk.write(1);
KDrainEE 0:2b269b9f417b 85 si.write(0);
KDrainEE 0:2b269b9f417b 86 clk.write(0);
KDrainEE 0:2b269b9f417b 87
KDrainEE 0:2b269b9f417b 88 //Pixel Count and read
KDrainEE 0:2b269b9f417b 89 for (int i = 0; i <128; i++)
KDrainEE 0:2b269b9f417b 90 {
KDrainEE 0:2b269b9f417b 91 wait_us(50); //saturation time
KDrainEE 0:2b269b9f417b 92 PixArray[i] = cameraIn1.read();
KDrainEE 0:2b269b9f417b 93 clk.write(1);
KDrainEE 0:2b269b9f417b 94 clk.write(0);
KDrainEE 0:2b269b9f417b 95 }
KDrainEE 0:2b269b9f417b 96
KDrainEE 0:2b269b9f417b 97 maxVal = 9;
KDrainEE 0:2b269b9f417b 98 for(int i = 10; i < 118; i++){
KDrainEE 0:2b269b9f417b 99 if(PixArray[i] > PixArray[maxVal]){
KDrainEE 0:2b269b9f417b 100 maxVal = i;
KDrainEE 0:2b269b9f417b 101 }
KDrainEE 0:2b269b9f417b 102 }
KDrainEE 0:2b269b9f417b 103 maxVal = maxVal- 64;
KDrainEE 0:2b269b9f417b 104 myled = !myled;
KDrainEE 0:2b269b9f417b 105 bt.printf("%f, ",data[0]);
KDrainEE 0:2b269b9f417b 106 bt.printf("%f, ", data[1]);
KDrainEE 0:2b269b9f417b 107 bt.printf("%f, ", data[2]);
KDrainEE 0:2b269b9f417b 108 bt.printf("%f\r\n", data[3]);
KDrainEE 0:2b269b9f417b 109 }
KDrainEE 0:2b269b9f417b 110 }