Shih-Ho Hsieh / Mbed 2 deprecated pololu5mag_with_platform

5 sensors on a board with a moving platform.

Dependencies:   mbed

Fork of Motor_XYZ_UI_SPI_I2C_5mag by Shih-Ho Hsieh

ui.cpp@9:c4f7257dee47, 2017-12-12 (annotated)

Committer:
hober
Date:
Tue Dec 12 09:37:33 2017 +0000
Branch:
envelope
Revision:
9:c4f7257dee47
Parent:
8:33d34a775873
Child:
10:a49cdab3810f
20171212 interrupt applied

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hober 6:ce02d396c961 1 #include "motor.h"
hober 6:ce02d396c961 2 #include "xyz_sensor_platform.h"
hober 8:33d34a775873 3 //#include "ParseArray.h"
hober 8:33d34a775873 4 #include "envelopetracker.h"
hober 6:ce02d396c961 5
hober 7:ee0569d49c52 6 #define I2C_FREQUENCY 400000
hober 6:ce02d396c961 7
hober 8:33d34a775873 8 typedef unsigned char byte;
hober 8:33d34a775873 9 uint8_t** dataToSend;
hober 8:33d34a775873 10 int sendArrayIndex = 0;
hober 8:33d34a775873 11 int sendBufferMax = 10000;
hober 8:33d34a775873 12 const int BAUD = 921600;
hober 8:33d34a775873 13 Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
hober 7:ee0569d49c52 14 DigitalOut led(LED2),led3(LED3);
hober 7:ee0569d49c52 15 DigitalOut mag_test(D11);
hober 6:ce02d396c961 16 XYZSensorPlatform platform;
hober 8:33d34a775873 17 Envelope *command;
hober 8:33d34a775873 18 EnvelopeTracker tracker;
hober 7:ee0569d49c52 19 byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
hober 7:ee0569d49c52 20 bool isRecording = false;
hober 9:c4f7257dee47 21 Envelope* result;
hober 9:c4f7257dee47 22 float x, y, z;
hober 9:c4f7257dee47 23 float pos[3];
hober 9:c4f7257dee47 24 int n = 0;
hober 9:c4f7257dee47 25 int getMag = 0;
hober 9:c4f7257dee47 26 int leftCount = 0;
hober 9:c4f7257dee47 27 int rightCount = 0;
hober 9:c4f7257dee47 28 int upCount = 0;
hober 9:c4f7257dee47 29 int downCount = 0;
hober 9:c4f7257dee47 30 int forwardCount = 0;
hober 9:c4f7257dee47 31 int backwardCount = 0;
hober 9:c4f7257dee47 32 bool commandToDo = false;
hober 9:c4f7257dee47 33 int recordTime;
hober 9:c4f7257dee47 34 float waitTime;
hober 8:33d34a775873 35
hober 9:c4f7257dee47 36 const int Fs = 1000; // sampling rate -- max: 1kHz
hober 8:33d34a775873 37
hober 8:33d34a775873 38
hober 6:ce02d396c961 39 void echo(char typ, float x, float y, float z);
hober 6:ce02d396c961 40 void echo(char typ, int16_t *p_data);
hober 9:c4f7257dee47 41 void Rx_interrupt();
hober 8:33d34a775873 42
hober 6:ce02d396c961 43 int main()
hober 6:ce02d396c961 44 {
hober 8:33d34a775873 45 dataToSend = new uint8_t*[sendBufferMax];
hober 6:ce02d396c961 46 led=1;
hober 8:33d34a775873 47 command = new Envelope;
hober 8:33d34a775873 48 command->enableHeader(std::string("H"));// 48 H
hober 8:33d34a775873 49 command->enableFooter(std::string("E"),8);// 45 E
hober 8:33d34a775873 50 command->enableCheckXOR(9);
hober 8:33d34a775873 51 tracker.setEnvelope(*command);
hober 8:33d34a775873 52 tracker.setBufferLength(100);
hober 7:ee0569d49c52 53 pc.format(8,SerialBase::None,1);
hober 6:ce02d396c961 54 platform.set_speed(2.5);
hober 7:ee0569d49c52 55 platform.reset(); // need to be modified here
hober 7:ee0569d49c52 56 platform.setSensorI2cFrequency(I2C_FREQUENCY);
hober 9:c4f7257dee47 57 // Setup a serial interrupt function to receive data
hober 9:c4f7257dee47 58 pc.attach(&Rx_interrupt, Serial::RxIrq);
hober 6:ce02d396c961 59 while(1) {
hober 8:33d34a775873 60 if(isRecording && n < recordTime) {
hober 7:ee0569d49c52 61 int16_t mag[3];
hober 7:ee0569d49c52 62 if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
hober 7:ee0569d49c52 63 echo('M',mag);
hober 7:ee0569d49c52 64 mag_test=!mag_test;
hober 8:33d34a775873 65 wait(waitTime);
hober 7:ee0569d49c52 66 }
hober 7:ee0569d49c52 67 n++;
hober 8:33d34a775873 68 } else if(isRecording) {
hober 8:33d34a775873 69 isRecording = false;
hober 8:33d34a775873 70 echo('S',0,0,0);
hober 8:33d34a775873 71 }// end recording if
hober 9:c4f7257dee47 72 if(!isRecording && n != 0) {
hober 9:c4f7257dee47 73 n = 0;
hober 9:c4f7257dee47 74 for(int i = 0; i < sendBufferMax; i++)
hober 9:c4f7257dee47 75 if(dataToSend[i]) delete dataToSend[i];
hober 9:c4f7257dee47 76 }
hober 9:c4f7257dee47 77 if(getMag>0) {
hober 9:c4f7257dee47 78 int16_t mag[3];
hober 9:c4f7257dee47 79 if(platform.get_mag_raw(mag)==0) echo('M',mag);
hober 9:c4f7257dee47 80 getMag--;
hober 9:c4f7257dee47 81 }
hober 9:c4f7257dee47 82 if(commandToDo) {
hober 9:c4f7257dee47 83 platform.to(x,y,z);
hober 9:c4f7257dee47 84 platform.position(pos);
hober 9:c4f7257dee47 85 echo('O',pos[0],pos[1],pos[2]);
hober 9:c4f7257dee47 86 commandToDo = false;
hober 9:c4f7257dee47 87 }
hober 9:c4f7257dee47 88 if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
hober 9:c4f7257dee47 89 if(leftCount > 0) {
hober 9:c4f7257dee47 90 platform.go_left();
hober 9:c4f7257dee47 91 leftCount--;
hober 9:c4f7257dee47 92 }
hober 9:c4f7257dee47 93 if(rightCount > 0) {
hober 9:c4f7257dee47 94 platform.go_right();
hober 9:c4f7257dee47 95 rightCount--;
hober 9:c4f7257dee47 96 }
hober 9:c4f7257dee47 97 if(upCount > 0) {
hober 9:c4f7257dee47 98 platform.go_up();
hober 9:c4f7257dee47 99 upCount--;
hober 9:c4f7257dee47 100 }
hober 9:c4f7257dee47 101 if(downCount > 0) {
hober 9:c4f7257dee47 102 platform.go_down();
hober 9:c4f7257dee47 103 downCount--;
hober 9:c4f7257dee47 104 }
hober 9:c4f7257dee47 105 if(forwardCount > 0) {
hober 9:c4f7257dee47 106 platform.go_forward();
hober 9:c4f7257dee47 107 forwardCount--;
hober 9:c4f7257dee47 108 }
hober 9:c4f7257dee47 109 if(backwardCount > 0) {
hober 9:c4f7257dee47 110 platform.go_backward();
hober 9:c4f7257dee47 111 backwardCount--;
hober 9:c4f7257dee47 112 }
hober 9:c4f7257dee47 113 platform.position(pos);
hober 9:c4f7257dee47 114 echo('O',pos[0],pos[1],pos[2]);
hober 9:c4f7257dee47 115 }
hober 7:ee0569d49c52 116 } // end while
hober 6:ce02d396c961 117
hober 6:ce02d396c961 118 }
hober 6:ce02d396c961 119
hober 6:ce02d396c961 120 void echo(char typ,float x, float y, float z)
hober 6:ce02d396c961 121 {
hober 7:ee0569d49c52 122 int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)};
hober 7:ee0569d49c52 123 echo(typ,p_data);
hober 6:ce02d396c961 124 }
hober 6:ce02d396c961 125
hober 6:ce02d396c961 126 void echo(char typ, int16_t *p_data)
hober 6:ce02d396c961 127 {
hober 8:33d34a775873 128 char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
hober 8:33d34a775873 129 command->setEnvelopeData(tmp,7);
hober 8:33d34a775873 130 dataToSend[sendArrayIndex] = (uint8_t*)(command->getEnvelopeArray());
hober 8:33d34a775873 131 for(int i = 0; i < command->length(); i++) {
hober 8:33d34a775873 132 pc.putc(dataToSend[sendArrayIndex][i]);
hober 8:33d34a775873 133 }
hober 8:33d34a775873 134 sendArrayIndex = (sendArrayIndex+1)%sendBufferMax;
hober 6:ce02d396c961 135 }
hober 6:ce02d396c961 136
hober 9:c4f7257dee47 137 void Rx_interrupt()
hober 9:c4f7257dee47 138 {
hober 9:c4f7257dee47 139 char c;
hober 9:c4f7257dee47 140 while(pc.readable()) {
hober 9:c4f7257dee47 141 c = pc.getc();
hober 9:c4f7257dee47 142 tracker.parse(&c,1);
hober 6:ce02d396c961 143
hober 9:c4f7257dee47 144 result = tracker.getEnvelope();
hober 9:c4f7257dee47 145 if(result!=NULL) {
hober 9:c4f7257dee47 146 char *dataArray = result->getPayload();
hober 9:c4f7257dee47 147 switch(dataArray[0]) {
hober 9:c4f7257dee47 148 case 'I':
hober 9:c4f7257dee47 149 break;
hober 9:c4f7257dee47 150 case 'O': // echo
hober 9:c4f7257dee47 151 platform.position(pos);
hober 9:c4f7257dee47 152 echo('O',pos[0],pos[1],pos[2]);
hober 9:c4f7257dee47 153 led3 = !led3;
hober 9:c4f7257dee47 154 break;
hober 9:c4f7257dee47 155 case 'C': // command
hober 9:c4f7257dee47 156 x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
hober 9:c4f7257dee47 157 y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 9:c4f7257dee47 158 z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 9:c4f7257dee47 159 commandToDo = true;
hober 9:c4f7257dee47 160 break;
hober 9:c4f7257dee47 161 case 'X':
hober 9:c4f7257dee47 162 if(dataArray[1]&0x80) rightCount++;
hober 9:c4f7257dee47 163 else leftCount++;
hober 9:c4f7257dee47 164 break;
hober 9:c4f7257dee47 165 case 'Y':
hober 9:c4f7257dee47 166 if(dataArray[1]&0x80) forwardCount++;
hober 9:c4f7257dee47 167 else forwardCount++;
hober 9:c4f7257dee47 168 break;
hober 9:c4f7257dee47 169 case 'Z':
hober 9:c4f7257dee47 170 if(dataArray[1]&0x80) upCount++;
hober 9:c4f7257dee47 171 else downCount++;
hober 9:c4f7257dee47 172 break;
hober 9:c4f7257dee47 173 case 'M': // magnet
hober 9:c4f7257dee47 174 getMag++;
hober 9:c4f7257dee47 175 // pc.putc(dataArray[0]);
hober 9:c4f7257dee47 176 // echo('O',mag);
hober 9:c4f7257dee47 177 mag_test=!mag_test;
hober 9:c4f7257dee47 178 break;
hober 9:c4f7257dee47 179 case 'R': // record
hober 9:c4f7257dee47 180 recordTime = dataArray[1];
hober 9:c4f7257dee47 181 recordTime *= Fs;
hober 9:c4f7257dee47 182 waitTime = 1.0/Fs-0.0005-1/(BAUD/8/10);
hober 9:c4f7257dee47 183 if(waitTime < 0) waitTime = 0;
hober 9:c4f7257dee47 184 isRecording = true;
hober 9:c4f7257dee47 185 break;
hober 9:c4f7257dee47 186 case 'S': // stop
hober 9:c4f7257dee47 187 isRecording = false;
hober 9:c4f7257dee47 188 break;
hober 9:c4f7257dee47 189 default:
hober 9:c4f7257dee47 190 break;
hober 9:c4f7257dee47 191 } // end switch
hober 9:c4f7257dee47 192 // delete result;
hober 9:c4f7257dee47 193 delete dataArray;
hober 9:c4f7257dee47 194 result = NULL;
hober 9:c4f7257dee47 195 dataArray = NULL;
hober 9:c4f7257dee47 196 } // end result if
hober 9:c4f7257dee47 197 } // end parsing if
hober 9:c4f7257dee47 198 }
hober 9:c4f7257dee47 199