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@8:33d34a775873, 2017-12-12 (annotated)

Committer:
hober
Date:
Tue Dec 12 08:47:56 2017 +0000
Branch:
envelope
Revision:
8:33d34a775873
Parent:
7:ee0569d49c52
Child:
9:c4f7257dee47
20171212 envelope accomplish sampling rate adjusted.; stopping method only by timer(a counter).

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 8:33d34a775873 21
hober 8:33d34a775873 22 const int Fs = 100; // sampling rate -- max: 1kHz
hober 8:33d34a775873 23
hober 8:33d34a775873 24
hober 6:ce02d396c961 25 void echo(char typ, float x, float y, float z);
hober 6:ce02d396c961 26 void echo(char typ, int16_t *p_data);
hober 8:33d34a775873 27
hober 6:ce02d396c961 28 int main()
hober 6:ce02d396c961 29 {
hober 8:33d34a775873 30 Envelope* result;
hober 6:ce02d396c961 31 float x, y, z;
hober 6:ce02d396c961 32 float pos[3];
hober 7:ee0569d49c52 33 int n = 0;
hober 8:33d34a775873 34 int recordTime;
hober 8:33d34a775873 35 float waitTime;
hober 8:33d34a775873 36 dataToSend = new uint8_t*[sendBufferMax];
hober 8:33d34a775873 37 // uint8_t *ch_in = new char [10];
hober 6:ce02d396c961 38 led=1;
hober 7:ee0569d49c52 39 mag_test=1;
hober 8:33d34a775873 40 command = new Envelope;
hober 8:33d34a775873 41 command->enableHeader(std::string("H"));// 48 H
hober 8:33d34a775873 42 command->enableFooter(std::string("E"),8);// 45 E
hober 8:33d34a775873 43 command->enableCheckXOR(9);
hober 8:33d34a775873 44 tracker.setEnvelope(*command);
hober 8:33d34a775873 45 tracker.setBufferLength(100);
hober 7:ee0569d49c52 46 pc.format(8,SerialBase::None,1);
hober 6:ce02d396c961 47 platform.set_speed(2.5);
hober 7:ee0569d49c52 48 platform.reset(); // need to be modified here
hober 7:ee0569d49c52 49 platform.setSensorI2cFrequency(I2C_FREQUENCY);
hober 8:33d34a775873 50 char c;
hober 6:ce02d396c961 51 while(1) {
hober 6:ce02d396c961 52 if(pc.readable()) {
hober 8:33d34a775873 53 c = pc.getc();
hober 8:33d34a775873 54 tracker.parse(&c,1);
hober 8:33d34a775873 55
hober 8:33d34a775873 56 result = tracker.getEnvelope();
hober 8:33d34a775873 57 if(result!=NULL) {
hober 8:33d34a775873 58 char *dataArray = result->getPayload();
hober 6:ce02d396c961 59 switch(dataArray[0]) {
hober 7:ee0569d49c52 60 case 'I':
hober 7:ee0569d49c52 61 break;
hober 6:ce02d396c961 62 case 'O': // echo
hober 6:ce02d396c961 63 platform.position(pos);
hober 6:ce02d396c961 64 echo('O',pos[0],pos[1],pos[2]);
hober 7:ee0569d49c52 65 led3 = !led3;
hober 6:ce02d396c961 66 break;
hober 6:ce02d396c961 67 case 'C': // command
hober 6:ce02d396c961 68 x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
hober 6:ce02d396c961 69 y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 6:ce02d396c961 70 z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 6:ce02d396c961 71 platform.to(x,y,z);
hober 6:ce02d396c961 72 platform.position(pos);
hober 6:ce02d396c961 73 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 74 break;
hober 6:ce02d396c961 75 case 'X':
hober 6:ce02d396c961 76 if(dataArray[1]&0x80)platform.go_right();
hober 6:ce02d396c961 77 else platform.go_left();
hober 6:ce02d396c961 78 platform.position(pos);
hober 6:ce02d396c961 79 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 80 break;
hober 6:ce02d396c961 81 case 'Y':
hober 6:ce02d396c961 82 if(dataArray[1]&0x80)platform.go_forward();
hober 6:ce02d396c961 83 else platform.go_backward();
hober 6:ce02d396c961 84 platform.position(pos);
hober 6:ce02d396c961 85 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 86 break;
hober 6:ce02d396c961 87 case 'Z':
hober 6:ce02d396c961 88 if(dataArray[1]&0x80)platform.go_up();
hober 6:ce02d396c961 89 else platform.go_down();
hober 6:ce02d396c961 90 platform.position(pos);
hober 6:ce02d396c961 91 echo('O',pos[0],pos[1],pos[2]);
hober 6:ce02d396c961 92 break;
hober 7:ee0569d49c52 93 case 'M': // magnet
hober 6:ce02d396c961 94 int16_t mag[3];
hober 6:ce02d396c961 95 if(platform.get_mag_raw(mag)==0) echo('M',mag);
hober 7:ee0569d49c52 96 mag_test=!mag_test;
hober 7:ee0569d49c52 97 break;
hober 7:ee0569d49c52 98 case 'R': // record
hober 8:33d34a775873 99 recordTime = dataArray[1];
hober 8:33d34a775873 100 recordTime *= Fs;
hober 8:33d34a775873 101 waitTime = 1.0/Fs-0.0005-1/(BAUD/8/10);
hober 8:33d34a775873 102 if(waitTime < 0) waitTime = 0;
hober 7:ee0569d49c52 103 isRecording = true;
hober 7:ee0569d49c52 104 break;
hober 7:ee0569d49c52 105 case 'S': // stop
hober 7:ee0569d49c52 106 isRecording = false;
hober 6:ce02d396c961 107 break;
hober 6:ce02d396c961 108 default:
hober 6:ce02d396c961 109 break;
hober 8:33d34a775873 110 } // end switch
hober 8:33d34a775873 111 // delete result;
hober 8:33d34a775873 112 delete dataArray;
hober 8:33d34a775873 113 result = NULL;
hober 8:33d34a775873 114 dataArray = NULL;
hober 8:33d34a775873 115 } // end result if
hober 7:ee0569d49c52 116 } // end parsing if
hober 8:33d34a775873 117 if(isRecording && n < recordTime) {
hober 7:ee0569d49c52 118 int16_t mag[3];
hober 7:ee0569d49c52 119 if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
hober 7:ee0569d49c52 120 echo('M',mag);
hober 7:ee0569d49c52 121 mag_test=!mag_test;
hober 8:33d34a775873 122 wait(waitTime);
hober 7:ee0569d49c52 123 }
hober 7:ee0569d49c52 124 n++;
hober 8:33d34a775873 125 } else if(isRecording) {
hober 8:33d34a775873 126 n = 0;
hober 8:33d34a775873 127 isRecording = false;
hober 8:33d34a775873 128 for(int i = 0; i < sendBufferMax; i++)
hober 8:33d34a775873 129 if(dataToSend[i]) delete dataToSend[i];
hober 8:33d34a775873 130 echo('S',0,0,0);
hober 8:33d34a775873 131 }// end recording if
hober 7:ee0569d49c52 132 } // end while
hober 6:ce02d396c961 133
hober 6:ce02d396c961 134 }
hober 6:ce02d396c961 135
hober 6:ce02d396c961 136 void echo(char typ,float x, float y, float z)
hober 6:ce02d396c961 137 {
hober 7:ee0569d49c52 138 int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)};
hober 7:ee0569d49c52 139 echo(typ,p_data);
hober 6:ce02d396c961 140 }
hober 6:ce02d396c961 141
hober 6:ce02d396c961 142 void echo(char typ, int16_t *p_data)
hober 6:ce02d396c961 143 {
hober 8:33d34a775873 144 // char * tmp = new char [7];
hober 8:33d34a775873 145 // tmp[0] = typ;
hober 8:33d34a775873 146 // tmp[1] = p_data[0]>>8;
hober 8:33d34a775873 147 // tmp[2] = p_data[0];
hober 8:33d34a775873 148 // tmp[3] = p_data[1]>>8;
hober 8:33d34a775873 149 // tmp[4] = p_data[1];
hober 8:33d34a775873 150 // tmp[5] = p_data[2]>>8;
hober 8:33d34a775873 151 // tmp[6] = p_data[2];
hober 8:33d34a775873 152 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 153 command->setEnvelopeData(tmp,7);
hober 8:33d34a775873 154 // if(dataToSend[sendArrayIndex]) delete dataToSend[sendArrayIndex];
hober 8:33d34a775873 155 dataToSend[sendArrayIndex] = (uint8_t*)(command->getEnvelopeArray());
hober 8:33d34a775873 156 for(int i = 0; i < command->length(); i++) {
hober 8:33d34a775873 157 // pc.write(dataToSend[sendArrayIndex],command->length(),NULL);
hober 8:33d34a775873 158 // wait(0.0002);
hober 8:33d34a775873 159 pc.putc(dataToSend[sendArrayIndex][i]);
hober 8:33d34a775873 160 }
hober 8:33d34a775873 161 sendArrayIndex = (sendArrayIndex+1)%sendBufferMax;
hober 8:33d34a775873 162 // delete dataToSend;
hober 8:33d34a775873 163 // dataToSend = NULL;
hober 6:ce02d396c961 164 }
hober 6:ce02d396c961 165
hober 6:ce02d396c961 166